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ABSTRACT 


Underwater  walking  machines  offer  a  potential  for  replacement  of  human  divers  in 
certain  aspects  of  underwater  construction  and  inspection.  One  such  vehicle,  Aquarobot, 
is  currently  under  test  in  Japan.  However,  this  vehicle  is  currently  too  slow  to  be 
economically  utilized,  and  limited  hardware  availability  restricts  progress  in  control 
software  improvements.  A  software  dynamic  simulation  model  is  desirable  to  relieve  this 
restricted  access.  The  problem  addressed  by  this  research  is  the  modeling  of  system 
dynamics  of  underwater  walking  vehicles  with  sufficient  simplification  to  achieve  a 
real-time  simulation.  The  approach  taken  includes  an  object-oriented,  massless  leg  robot 
dynamic  model  and  employs  a  high  performance  graphics  rendering  toolkit. 

The  resulting  simulations  of  a  robotic  joint  actuator  and  of  the  robot  itself  utilizing 
springs  and  dampers  in  the  joints,  runs  in  real-time.  The  robot  simulation  model  executes 
on  a  four-processor  nuichine  with  under  fifteen  percent  utilization  of  the  processor 
dedicated  to  system  dynamics.  This  result  indicates  that  the  simulation  is  likely  to  retain 
real-time  capability  after  replacing  the  springs  and  dampers  with  the  more  accurate  joint 
actuator  model  also  developed  in  this  theas. 
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I.  INTRODUCTION 


A.  AQUAROBOT 

Aquarobot  is  a  six-legged  "insect  type",  articulated,  experimental  prototype  robot 
under  development  at  the  Port  and  Harbour  Research  Institute  (PHRI)  in  Japan.  Tlus 
robot  is  being  investigated  as  an  alternative  to  the  currently  used  human  divers  for  seawall 
construction  and  inspection.  While  the  divers  are  fully  capable,  the  limited  stay  time  at 
required  depths  make  progress  slow  and  expensive.  A  walking  robot  is  preferred  to 
tracked,  wheeled,  or  floating  versions  for  its  abilities  to  maneuver  without  disturbing  the 
bottom  enough  to  cloud  the  water  and  restrict  viability,  and  to  provide  a  stable  rrference 
platform  from  which  measurements  can  be  made  [Iwasaki,  1987].  The  prototype 
Aquarobot  has  successfully  walked  underwater  and  demonstrated  functional  feasibility  for 
the  intended  task,  but  it  is  currently  too  slow  to  be  economically  utilized  [Davidson, 
1993].  The  Naval  Postgraduate  School  (NFS)  is  working  with  PHRI  to  upgrade 
Aquarobot's  control  software,  from  the  original  varaon  written  in  BASIC,  to  improve  its 
operating  speed. 

B.  GOALS 

The  goal  of  this  thesis  is  to  investigate  the  feasibility  of  dynamic  modeling  of 
Aquarobot  with  sufiGciem  simplifications  to  achieve  a  real-time  simulation.  The  Emulation 
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model  should  be  statically  accurate  and  dynamically  a{>proxim8te.  The  nuyor 
simplifications  considered  are; 

(1)  massless  legs, 

(2)  body  mass  evenly  distributed  in  a  cylinder, 

(3)  center  of  mass  at  geometric  center  of  the  inboard  leg  joints, 

(4)  infinite  fiiction  for  foot  contact  with  surface. 

The  dynamic  Aquarobot  model  of  this  thesis  uses  springs  and  dampers  in  place  of 
joint  actuators  for  this  initial  feasibility  study.  A  joint  actuator  simulation  model,  including 
servomotor  and  controller  models,  is  also  developed  and  is  intended  to  eventually  replace 
the  springs  and  dampers.  Inputs  to  that  model  will  be  control  software  orders  to  the  joint 
motor  controller. 

C.  ORGANIZATION 

Chapter  n  of  this  thesis  reviews  previous  and  concurrent  work  in  the  area  of  walking 
robots  with  emphasis  on  work  related  to  Aquarobot.  Chapter  m  provides  a  more  detailed 
description  of  Aquarobot  and  introduces  the  software  tools  used.  Chapters  IV  and  V 
develop  the  necessary  mathematical  models  and  then  present  prototype  dynamic 
simulation  models  for  an  Aquarobot  joint  actuator  and  for  Aquarobot  itself  (with  springs 
and  dampers  in  place  of  joint  actuators).  Chapter  VI  reviews  the  results  of  simulations 
accomplished  with  the  models  introduced  in  Chapters  IV  and  V.  Finally,  Chapter  Vn 
presents  some  conclusions,  suggestions  for  fiirther  research,  and  a  summary. 
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II.  SURVEY  OF  PREVIOUS  AND  CONCURRENT  WORK 


A.  INTRODUCTION 

To  place  Aquarobot  research  in  relative  perspective,  this  chapter  begins  with  a  brief 
historical  review  of  walking  machines.  An  overview  of  ongoing  Aquarobot  research  at 
NPS  follows  and  places  this  thesis  in  context.  Other  contributions,  some  completed  and 
some  currently  in  progress,  are  described.  Also,  as  this  thesis  is  a  continuation  of  previous 
work,  a  short  review  of  some  of  the  key  elements  of  that  work  is  presented  to  provide  a 
starting  frame  of  reference. 

B.  BRIEF  HISTORY  OF  WALKING  MACHINES 

In  an  early  exploration  of  walking  mechanisms,  1965  to  1968,  General  Electric 
Corporation  built  a  four  legged  vehicle  called  the  Quadruped  Transporter.  Because  of  the 
lack  of  theory  and  technology,  designers  incorporated  human  sensing  and  neural  control  of 
the  limbs  by  attaching  "position  following,  force  feedback”  control  levers  to  the  operator's 
arms  and  legs.  Each  of  these  levers  had  three  degrees  of  freedom,  corresponding  to  those 
of  the  leg  it  controlled.  Very  few  mastered  the  skills  required  to  operate  the  vehicle,  and 
those  that  did  found  it  to  be  very  demanding  [McQiee,  1985].  While  the  Quadruped 
Transporter  lacked  practicality,  its  successful  implementation  encouraged  further  research. 
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Automation  of  low  level  tasks,  such  as  individual  limb  control,  leaves  the  operator 
free  to  concentrate  on  higher  level,  "supervisory  control"  of  the  vehicle.  In  1977,  this 
method  of  control  was  used  in  the  Ohio  State  University  (OSU)  "Hexapod  Vehicle."  The 
operator  controlled  vehicle  speed  and  direction,  using  a  joystick,  while  limb  motion 
control  and  coordination  was  handled  by  computer.  This  machine  was  utilized  in  the 
development  of  gait  algorithms  [McGhee,  1985]. 

[McGhee,  1986]  addresses  the  energy-efiSciency  issue  of  limb  control  and  introduces 
a  method  used  to  achieve  a  cyclic  leg  motion  without  requiring  the  reversal  of  drive 
motors.  This  approach  was  demonstrated  in  MELCRABs  1  and  2  at  Mechanical 
Engineering  Laboratory  in  Japan. 

From  1981  to  1986,  the  Adaptive  Suspension  Vehicle  (ASV)  was  constructed  and 
tested  at  OSU.  The  ASV  was  a  six-legged  vehicle  designed  for  outdoor  operation  on 
irregular,  unmapped  terrain  and  included  a  self  contained,  onboard  power  supply.  It 
carried  an  operator  who  exercised  supervisory  control  via  a  joystick  and  keypad.  Leg 
coordination  and  foothold  selection  were  fiiUy  automated;  the  latter  was  allowed  by 
employment  of  extensive  environmental  sensors  including  an  optical  terrain  scanner 
[Waldron,  1986].  Related  later  work  [Kwak,  1990]  explores  the  use  of  rule-based  limb 
motion  coordination  to  implement  a  "free,"  non-periodic,  gait  permitting  on-line 
optimization  of  foot  placement. 
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C.  AQUAROBOT  RESEARCH  AT  NPS 

Aquarobot  research  at  NPS  is  divided  into  two  concurrent  phases;  control  and 
simulation.  The  first  of  these,  control,  consists  of  Aquarobot  control  software 
development.  The  second  phase,  simulation,  involves  development  of  a  graphical 
computer  simulation  of  the  Aquarobot  hardware.  The  simulator  is  required  for  the  final 
stages  of  the  control  software  development,  including  testing. 

1.  Control  Software  Development 

While  final  development  and  testing  of  control  software  depends  the  availability 
of  the  simulation  model,  some  work  has  been  accomplished  prior  to  such  availability.  In 
[Schue,  1993]  an  algorithm  is  presented  for  statically  stable,  alternating  tripod  gait 
planning  and  foot  path  planning  for  smooth  leg  motion  during  walking  on  flat  terrain. 
Further  developments  in  gait  planning  algorithms  and  demonstration  of  alternative  gaits, 
which  allow  variable  direction  and  speed  but  require  continuous  adjustment  of  leg  liftoff 
and  touchdown  sequence,  are  reviewed  in  [Yoneda,  1993], 

2.  Simulation  Software  Development 

The  framework  for  the  Aquarobot  simulation  model  is  provided  in  [Davidson, 
1993],  in  which  an  object-oriented  kinematic  model  is  developed.  Both 
Danevit-Hartenberg  (DH)  and  Craig  (Modified  Danevit-Hartenberg  or  MDH)  methods  are 
presented  and  then  compared.  The  fundamental  difference  in  the  two  methods  is  in  the 
coordinate  systems  used  for  a  "link,"  the  rigid  limb  component  between  two  joints.  The 
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DH  methodology  utilizes  the  outboard,  closer  to  end<effector,  joint  as  the  coordinate 
system  origin  while  the  Craig  method  uses  the  inboard,  closer  to  body,  joint.  The  Craig 
version  of  Davidson's  kinematic  model  is  used  in  this  thesis. 

[Goetz,  1994]  explores  a  variety  of  enhancements  for  the  Aquarobot  simulation 
model.  A  graphics  model,  which  incorporates  a  surrounding  operating  environment 
(terrain),  is  developed  to  replace  the  original  stick  figure.  The  mudel  includes  I/O  control 
interfaces  (i.e.  keyboard,  joystick,  spaceball)  and  foot/ground  collision  detection. 

A  complete,  unsimpUfied  physical  dynamic  simulation  model  of  Aquarobot, 
including  the  hydrodynamic  forces  of  its  operating  environment,  is  also  being  developed 
[McMillan,  1993].  While  this  simulation  is  not  expected  to  run  in  real  time,  it  will  provide 
valuable  data  for  comparison  to  the  simplified  model. 

D.  REUSED  SOFTWARE 

As  mentioned  above,  the  Aquarobot  simulation  presented  in  this  thesis  is  based  on  the 
model  described  by  [Davidson,  1993].  A  summary  of  the  key  features  of  that  model  is 
provided  here  for  quick  reference. 

1.  Rigid  Body  Qass 

In  both  LISP  and  C++  versions  of  the  Aquarobot  model,  system  dynamics  for  six 
degrees  of  fireedom,  three  translatiorud  and  three  rotational,  are  handled  within  a  "rigid 
body  class"  fi-om  [Davidson,  1993].  System  state  variables  include  world  coordinate 
position  and  orientation,  stored  in  a  4x4  "body  to  world"  coordinate  transformation 
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matrix,  called  the  "H-matrix**,  and  velocities  in  body  coordinates.  Euler  integrations  are 
used  for  dynamic  updates.  Acceleration  equations  in  body  coordinates  are; 


i  =  vr-wqr+§-^sine; 

(2.1) 

v  =  Hp-ur+^+gcos0  sin<|»; 

(2.2) 

w-uq-vp^^ii+g  COS0  cos4> ; 

(2.3) 

•  W»~lu)qr^L\ 

P-  /- 

(2.4) 

(2.5) 

•  ((/»-4y)|P<f+V) 

(2.6) 

where  m  is  body  mass;  g  is  gravitational  acceleration,  in  world  coordinates;  and  /„ 
are  the  moments  of  inertia;/  =  is  the  vector  of  applied  forces;  T=  (L,  M,  N)  is  the 

vector  of  applied  torques;  theta  and  ptu  are  Euler  pitch  and  roil  angles,  respectively;  u,  v, 
w  are  the  components  of  translational  velocity;  and  p,  q,  r  are  rotational  rate  componmts 
[Frank,  1969],  With  a  single  exception,  g,  the  above  values  are  expressed  in  body 
coordinates.  The  dynamic  update  is  achieved  by  determining  incremental  position  and 
orientation  changes,  in  body  coordinates,  and  uang  those  to  generate  an  incremental 
motion  matrix  which  is  then  post-multiplied  with  the  body's  H-matrix,  and  using  that  result 
to  update  (replace)  the  H-matrix.  Euler  integrations  and  equations  2.1  through  2.6  are 
used  to  update  the  velocity  state  variables  [Davidson,  1993]. 


7 


2.  Kinematic  Model 


Each  Aquarobot  limb  (leg)  is  made  up  of  a  series  of  links:  the  inboard  end  of  the 
series  being  physically  connected  to  the  robot's  body  and  the  outboard  end  the  foot.  Using 
the  Craig  method,  a  coordinate  system  at  the  outboard  end  of  a  link  is  described  relative  to 
the  coordinate  system  at  the  inboard  end  by  a  transformation  matrix  called  a  "T-matrix". 
The  T-matrix  depends  on  the  physical  construction  of  the  link  and,  in  the  case  of  rotary 
links,  the  rotation  angle  of  the  inboard  joint.  The  ori^  of  each  such  coordinate  system  is 
the  position  of  the  joint  between  two  such  links,  and  the  z-axis  is  aligned  so  that  a  joint 
rotation  is  a  z-axis  rotation.  The  entire  system  is  kept  in  a  hierarchical  structure  of  "rigid 
bodies",  with  H-matrix  updates  done  from  the  top  down:  i.e.  a  link's  H-matrix  may  be 
updated  only  after  the  link  next  inboard  is  updated  (the  robot  body  in  the  case  of  the 
inboard  end  of  each  leg)  [Davidson,  1993]. 

[H.]  =  [H,J  [TJ  (2.7) 

E.  SUMMARY 

The  development  of  the  Aquarobot  simulation  model  will  directly  support  final 
development  and  testing  of  Aquarobot  control  software.  The  dynamic  model  developed  in 
this  thesis  is  based  on  a  previously  developed  kinematic  model.  Before  describing  the 
dynamic  model,  a  more  detailed  description  of  Aquarobot  and  an  overview  of  the  software 
tools  used  is  needed  and  is  provided  in  Chapt^  m. 
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in.  DESCRIPTION  OF  AQUAROBOT  VEHICLE  AND 
SIMULATION  ENVIRONMENT 


A.  INTRODUCTION 

This  chapter  provides  a  physical  description  of  Aquarobot,  including  some  details  that 
are  beyond  the  scope  of  models  developed  in  this  thesis.  Special  emphasis  is  given  to  the 
joint  actuators  as  they  are  the  primary  feature  to  be  modeled.  In  addition,  the  software 
tools  used  in  the  development  are  introduced. 

B.  PHYSICAL  DESCRIPTION  OF  AQUAROBOT 

Figure  3.1  is  a  photograph  of  Aquarobot  which  has  a  body,  that  is  generally 
cylindrical  in  shape,  and  six  legs,  equally  spaced  at  sixty  degrees  apart.  Mounted  on  top  of 
the  body  is  a  camera  boom  with  three  rotary  joints  for  positioning.  The  boom  is  equipped 
with  an  ultrasonic  ranging  device  to  assist  in  the  scaling  of  measurements.  Within  the 
body  are  two  inclinometers  (for  attitude  sen^g),  a  gyrocompass,  and  a  depth  sensing 
device.  Aquarobot  is  computer  controlled  from  the  surfiice  via  a  four  centimeter  diameter 
tether  cable  attached  to  the  top  of  the  body.  The  cable  carries  control  signals  to  the  robot 
and  returns  sensor  signals  back  to  the  controlling  computer. 

Each  of  the  six  identical  insect  type  legs  has  three  rotary  joints,  which  are  driven  by 
the  joint  actuators  described  in  the  next  section,  and  a  freely  rotating  ball  joint  to  attach 
the  disc  shaped  foot.  Each  foot  has  a  pressure  sensitive  touch  sensor  to  provide  an 
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indication  of  ground  contact.  Figure  3.2  illustrates  the  axis  of  rotation  for  each  joint  in  an 
Aquarobot  leg. 


Figure  3.1 

Photograph  of  Aquarobot 


Fgure3.2 

Aquarobot  Leg  Construction 
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C.  AQUAROBOT  JOINT  ACTUATORS 

Figure  3.3  is  a  block  diagram  of  an  Aquarobot  joint  actuator.  Control  software  sends 
incremental  rotation  orders  to  the  controller  as  pulses,  with  polarity  indicating  the  desired 
direction  of  rotation.  Motor  response  is  fed  back  to  the  control  software  as  pulses  in  the 
same  manner.  These  feedback  pulses  are  produced  by  a  pulse  generator  on  the  motor 
shaft.  Additional  signals  to  and  from  the  control  software  include  pulse  counter  (PC) 
overflow,  pulse  counter  clear,  and  driver  enable. 


The  difference  between  the  actual  and  ordered  position,  the  error  signal  of  the  motor, 
is  kept  in  the  pulse  counter.  Orders  increment  the  counter,  phis  or  minus,  depending  on 
dewed  direction;  re^onse  pulses  decrement  it.  One  hundred  pulses  are  required  for  one 
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motor  revolution  (determined  by  the  nmtor  shaft  pulse  generator  output).  The  counter 
overflows  if  the  maximum  count,  +/-  6 144  pulses,  is  exceeded. 

A  digital  to  analog  converter  (D/A)  produces  a  DC  signal  directly  proportional  to  the 
current  count  in  the  pulse  counter,  and  its  output  is  the  motor  driver's  prinuiiy  input.  That 
is.  the  larger  the  error,  the  higher  the  vokage  applied  to  the  motor  to  correct  the  error.  A 
maximum  count  of  +/-6144  is  converted  to  +/-  lOVDC,  i.e. 

DAou,  =  count  *  (10/6144).  (3.1) 

A  high  gain  for  the  error  signal  is  desirable  for  fast  response  and  for  sufficient 
response  to  small  errors.  However,  by  itself  the  high  gain  would  cause  severe  overshoot 
and  oscillations  in  the  motor.  Degenerative  feedback,  proportional  to  motor  speed,  is 
used  to  prevent,  or  at  least  minimize,  this  overshoot.  This  degenerative  feedback  is 
provided  by  the  frequency  to  vohage  converter  (F/V)  r^ch  monitors  the  pulse  generator 
output.  The  output  is  3  VDC  per  thousand  RPhTs,  so 

FV^  =  RPM*{y\(m)  (3.2) 

The  driver  amplifies  outputs  from  the  D/A  and  F/V  converters  to  produce  the  source 
vohage  ( V^)  for  the  servomotor.  So  flu,  we  have 

V,  =  GxDAam-G2FVo^,  (3.3) 

where  Gi  and  G,  are  re^ecttve  gains.  Howevo’,  the  driver  is  actually  more  complex  and 
includes  an  additional  internal  fi^dback  path. 

The  signals  form  the  D/A  and  F/V  converters  are  amplified  and  fed  into  the  pulse 
width  modulator  (PWM)  with  the  F/V  signal  insetted  between  inverting  amplifiers  to 
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provide  degeneration.  A  current  pickup  on  the  PWM  output  line  provides  a  signal 
proportional  the  current  drawn  by  the  motor,  and  therefore  proportional  to  the  motor 
torque.  Recall  that  for  a  given  voltage  applied  to  a  motor,  it  has  a  limited  speed  due  to 
back  EMF.  As  the  motor  approaches  that  speed,  it  draws  less  current.  This  current 
(torque)  feedback  signal  provides  regenerative  feedback  in  the  driver  during  motor 
acceleration,  thus  reducing  the  response  time.  Adding  the  torque  feedback  to  Equation 
3.3  yields 

Vs  =  G\DAota-G^FVout■^G■sIa,  (3.4) 

where  /„  represents  the  motor's  armature  current.  Actually,  this  equation  still  neglects 
some  complexities  in  the  Aquarobot  joint  controller.  Not  shown  in  Figure  3.3  are 
feedback  capacitors  around  amplifiers  which  further  modify  the  equation  for  V^.  These 
effects  are  not  modeled  in  the  work  of  this  thesis. 

The  motor  is  driven  by  a  squarewave  rather  than  a  DC  vohage.  Figure  3.4  illustrates 
idealized  agnals.  The  function  of  the  PWM  is  to  provide  a  squarewave  of  constant 
amplitude,  zero  to  +/-  75  vohs,  with  a  duty  cycle  proportional  to  the  input  signal,  so  V\  in 
Equation  3.4  is  actually  an  average  value.  Motor  response  to  this  signal  is  very  close  to  its 
response  to  a  DC  vohage  equal  to  the  average  vohage  of  the  squarewave.  Applying 
Signals  1  or  2  of  Figure  3.4  to  the  motor  is  thus  equivalent  to  applying  +2SVDC  or 
-2SVDC,  re^ectively.  This  methodology  is  used  primarily  for  hs  efficiency  advantage 
over  a  DC  linear  arrqrlifier  [Truxal,  1958]. 
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Figure  3.4 

PWM  Outputs  For  33  %  Duty  Cycles 

Attached  to  each  servomotor  is  a  harmonic  reduction  gear,  built  together  as  a  single 
unit.  The  model  used  for  joint  one  in  each  leg,  RA-20,  has  a  reduction  ratio  of  161:1, 
while  that  used  for  joints  two  and  three,  RH>2S,  has  a  160: 1  ratio,  hi  addition,  joints  two 
and  three  have  a  bevel  gears  cascaded  with  the  harmonic  gears  with  3: 1  and  2:1  ratios, 
respectively.  Figure  3. 5  summarizes  the  total  reduction  ratios  and  gives  the  control  pulses 
required  for  a  single  joint  shaft  revolution. 


Joint 

1 

2 

3 

Harmonic  Gear 

161:1 

160:1 

160:1 

Bevel  Gear 

NA 

3:1 

2:1 

Total  Gear  Ratio 

161:1 

480:1 

320:1 

Pulses  /  Revolution 

16,100 

48,000 

32,000 

Figure  3.3 

Total  Reduction  Gear  Ratios  and 
Control  Pulse  Requirement  per  Joint  Revolution 
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D.  SOFTWARE  TOOLS 

C++  is  the  programmiiig  language  selected  for  moddmg  Aquan^KM.  This  choice  was 
based  on  both  hardware  and  software  conaderations.  IRIS  Performer,  a  C/C++  visual 
simulation  toolkit,  provides  rendering  that  is  fiist  enough  to  display  a  real  time  dynamic 
simulation  [Goetz,  1994].  Common  LISP  was  used  for  rapid  prototyping  and  testing 
during  the  early  stages  of  development. 

1.  C++ 

In  a  long  term  project  with  many  contributors,  object  oriented  design  provides  a 
high  level  of  abstraction  which  promotes  rapid  system  level  comprdiension  by  new  team 
members  as  well  as  loosely  coupled,  easily  maintained  source  code.  Not  only  does  C++ 
suppoit  the  object  oriented  design  paradigm,  but  also,  it  is  based  on,  and  includes  as  a 
subset,  the  highly  efficient,  structured  language,  C  [Wiener,  1988]. 

The  simulation  platform,  both  here  and  at  PHRl,  is  an  IRIS  Reality  Engine.  The 
C/C++  compilers  delivered  with  the  IRIS  systems  are  very  efficient,  due  to  direct  access  to 
the  low  level  hardware,  and  are  intended  to  be  used  as  native  developn^nt  languages. 

Popularity  and  wide^read  use  make  C/C++  source  code  portable,  and  while 
portability  is  not  a  key  issue  for  the  Aquarobot  simulation,  it  is  very  much  so  for  the 
Aquarobot  control  software  >^ch  is  likefy  to  have  to  survive  hardware  upgrades. 


15 


2.  Performer 


IRIS  Performer  is  used  primarily  for  its  performance  as  a  rendering  tool 
Performer  is  a  hardware  oriented,  C/C++-  graphics  tool  kh  deagned  to  operate  on  Silicon 
Graphics  products.  Its  routines  take  full  advantage  of  the  "known  hardware"  platform  to 
allow  much  higher  performance  than  routines  written  for  generic  hardware.  Also,  some 
precision  is  traded  for  ^eed  as  real  numbers  in  Performer  data  structures  are  single 
precision  "floats"  rather  than  "doubles".  Upon  initialization.  Performer  detects  hardware 
capability  and  automatically  sets  up  a  muhiprocessmg  environment  with  shared  memory 
when  running  on  a  muh^rocessor  machine.  The  de&uh  configuration,  which  the  user 
may  override,  is  separate  processors  assigned  for  (1)  user  application,  (2)  graphics 
database  culling,  and  (3)  drawing  the  culled  database  to  a  graphics  window  [SGI,  1992]. 

Peiformei's  graphics  database  is  stored  in  a  hierardiical  data  structure,  a  tree. 
The  structure  is  culled  and  rendered  by  doing  a  pre-order  traversal  with  child  nodes 
inheriting  the  accumulated  environmoits  (transformations)  of  all  ancestors  traversed  in  the 
current  path.  These  accumulated  environments  are  kept  on  a  stack  and  are  popped  off 
when  traversing  back  up  the  tree.  The  base  nodes  are  Scene  (the  root).  Static  Coordinate 
System  (SCS),  Dynamic  Coordinate  System  (DCS)  (variable  fi>r  motion  where  an  SCS  is 
fixed),  Groiq)  (a  container  for  children  requiring  a  common  transformation),  and  Geode  (a 
container  for  pofygons  to  be  rendered).  Geodes  imist  be  leaves  but  may  have  more  than 
one  parent.  This  reduces  the  memory  requirements  when  a  groiq)  of  polygons  is  to  be 
rendered  more  than  once  in  a  fi'ame  (two  or  more  identical  objects)  [SGI,  1992]. 
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Robotics  and  mechanics  users  must  be  aware  that  graphics  standards  are  used  in 
matrix  construction  and  operations.  The  graphics  matrix  is  the  transpose  of  the  standard 
robotics  matrix  [Fu,  1987],  GA/^  =  /?A^.  This  is  only  important  ^^en  using  the  pfMatrix 
data  type  and  operations  outside  those  that  directly  support  the  graphics  database,  or 
when  directly  manipulating  individual  elements,  rows,  and  /  or  columns.  Since  A/xiV  = 
the  order  of  matrix  muhiplications  may  also  have  to  be  reversed.  Lastly.  X  and 
Y  axis  rotations  are  as  expected;  however  in  Performer,  pitch  refers  to  an  X,  rather  than 
Y.  axis  rotation,  and  likewise,  roll  refers  to  a  Y  axis  rotations  [SGI,  1992], 

Synchronization  for  a  real-time  application  may  be  achieved  by  setting 
Performer's  "desired  frame  rate"  and  then  using  the  pfSync  function  call.  The  pfSync 
function  will  put  all  processes  to  sleep  between  each  frame  to  force  the  desired  frame  rate. 
If  the  desired  frame  rate  is  faster  than  can  be  achieved,  pfSync  will  have  no  effect,  and  the 
application  will  free  run  at  its  frstest  ^eed.  hi  the  Aquarobot  filiation,  the  frame  rate  is 
set  to  twenty  frames  per  second  and  then  a  fixed  deha-time  of  one  twentieth  of  a  second  is 
used  for  dynamic  updates.  Reading  the  internal  clock  for  deha-time  would  have  been 
equivalent  providing  the  application  is  capable  of  running  at  the  desired  frame  rate.  The 
fixed  deha-time  ensures  control  over  data  points  [SGI,  1992], 

3.  LISP 

The  problem  with  "rapid,  experimental"  prototyping  in  C/C++,  as  well  as  other 
compiled  languages,  is  the  difficulty  with  testing  and  verification.  Specifically,  a  test  shell 
must  be  wrhten,  compiled  and  run  to  thoroughly  test  a  block  of  source  code.  If  the  test 
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results  in  the  detection  of  errors,  debugging  tools  are  available,  but  the  source  code  must 
be  recompiled  to  include  them.  Other  methods,  such  as  insertion  of  additional  lines  of 
code,  are  also  available  but  also  require  multiple  compilations.  In  any  case,  logic  errors 
are  still  difScult  to  find  when  they  exist  in  large  blocks  of  code,  especially  when  they  only 
apply  to  specific  inputs.  LISP,  on  the  other  hand,  is  an  ideal  language  for  e?q)erimental 
work.  LISP  is  an  interpreted,  fimctional  language  which  gives  the  developer  the  ability  to 
call  any  defined  fimction  directly  from  the  command  prompt.  The  fimction's  return  value 
is  immediately  displayed  for  easy  comparison  to  expected  values;  test  routines  are  not 
required.  Since  fimctions  may  be  nested,  these  test  calls  may  include  any  desired  level  of 
abstraction.  Variables  are  also  directly  accessible  in  LISP  and  may  be  inspected  at  any 
time.  The  basic  structure  of  a  LISP  program  is  an  on-line  database  of  definitions: 
constants,  fimctions,  and  symbols  (pointers  to  variables)  [Koschmann,  1990]. 

Weak  typing  in  LISP  allows  additional  flexibility.  Typing  is  done  dynamically  at 
run  time.  For  exaiiq)le,  a  single  definition  of  the  fimction  "minimum”  may  be  used  for 
integers,  reals,  or  any  argument  type  for  which  the  operator  "<"  is  defined.  Variables, 
arguments,  and  return  values  may  also  be  lists,  allowing  the  developer  to  call  a  fimction 
with  a  coiiq)rehensive  set  of  test  inputs  [Koschmann,  1990]. 

The  Common  LISP  Object  System,  CLOS,  provides  full  support  for  object 
oriented  programming.  In  this  thesis,  a  prototype  deagn  using  CLOS  is  first  developed  to 
implement  system  dedgn  decisions.  Subsequent  translation  to  C++  is  then  accomplished 
without  abstract  structure  modifications. 
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E.  SUMMARY 


This  chapter  physically  describes  Aquarobot  with  the  intention  of  providing  sufficient 
orientation  prior  to  the  model  presentations.  Several  Aquarobot  features  are  not  included 
in  the  dynamic  simulation  model  presented  later  in  Chapter  V.  1  he  kinematic  model 
includes  the  body,  legs  and  feet  but  not  the  camera  boom  Joint  position,  foot  contact, 
and  azimuth  information  are  available;  however,  attitude  and  depth  are  not.  The  tether 
cable  and  hydrodynamic  forces,  currents  and  viscosity,  are  also  neglected. 

Also  in  preparation  for  the  model  presentations,  the  software  tools  utilized,  along 
with  the  reasons  for  their  selections,  are  introduced.  Object-oriented  system  design  and 
the  need  for  a  high  performance  made  C++  ard  IRIS  Performer  ideal  software  tools  for 
the  Aquarobot  simulation  model  CLOS,  with  its  capability  for  rapid  testing  of  complex 
functions,  was  used  for  prototyping  and  model  verification. 

The  following  chapters  present  dynamic  simulation  models  of  an  Aquarobot  joint 
actuator  and  Aquarobot  itself 
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IV.  JOINT  ACTUATOR  MODEL 


A.  INTRODUCTION 

The  purpose  of  this  chapter  is  to  present  a  smuilation  model  for  a  single  Aquarobot 
joint  actuator.  It  begins  with  a  review  of  the  basic  mathematics  required  to  model 
servomotors  and  reduction  gears  and  then  develops  the  joint  actuator  model. 

B.  BASIC  DC  MOTOR 

A  motor  is  a  device  used  to  convert  electrical  energy  into  mechanical  energy.  The 
force,  F,  on  a  current,  /,  carrying  conductor  of  length  /  in  a  uniform  magnetic  field,  B,  is 
given  by  [Halliday,  1981] 

F  =  \idlxB.  (4.1) 

If  the  conductor  is  fixed  on  a  shaft,  parallel  to  the  shaft,  at  radius  r,  the  resuking  torque, 
tau,  is  given  by  [Halliday,  1981] 

x=rxF.  (4.2) 

The  motor’s  armature  includes  a  set  of  such  conductors  and  is  usually  constructed  so  as  to 
be  symmetrical  with  reject  to  the  axis  of  rotation;  therefore,  the  total  conductor  length 
and  relative  position  (to  the  magnetic  field)  is  independent  of  the  armature's  angular 
position.  Furthermore,  if  a  constant  magnetic  field  (Le.  permanent  magnet  or  constant 
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current  electronugnet)  is  used,  then  the  force  and  resulting  torque  become  directly 
proportional  to  the  armature  current,  smq)lifying  the  above  equations  to 


F  =  /C,  (4.3) 

and 

Trf  =  iK„  (4.4) 

where  tau^  is  developed  torque,  and  torque  constant  K,  is  a  characteristic  of  the  motor. 

The  armature  current  depends  on  applied  voltage,  armature  resistance,  and  armature 
angular  velocity,  omega.  The  velocity  dependency  is  due  to  the  vohage  induced  in  the 
conductors  as  they  move  through  the  field  (Faraday's  Law).  Since  this  induced  voltage  is 
of  such  polarity  that  it  causes  a  decelerating  torque  (Lenz's  Law),  it  is  called  Back  or 
CounterEMF,  [Halliday,  1981]  [McHierson,  1981] 

Ffc  =  /:»©,  (4.5) 

where  is  the  motor's  back  EMF  constant. 

Armature  inductance  is  usually  negligible  for  high  quality  servomotors,  so  using  Ohm's 
Law,  the  armature  current,  /,,  is  given  by  [Halliday,  1981]  [McPherson,  1981] 


where  is  the  voltage  applied  to  the  armature  and  is  armature  resistance. 

Combining  Eqiutions  4.4  through  4.6,  the  motor's  developed  torque  is 

=  =  (4.7) 

Given  no  external  torques  and  ignoring  losses  for  now,  for  any  there  is  an  associated 
maximum  ^eed,  when  omega,  that  resuhs  in  tau^  =  0  and  therefore  no  fiuther 


acceleration.  Still  ignoring  losses,  motor  acceleration,  omega-dot.  is  given  by  the  standard 
rotational  dynamics  equation  [HaUiday,  1981] 


o  =  =  (4.8) 

where  is  internal  motor  inertia,  and  tau^  and  are  external  torque  and  inertia, 
respectively.  There  are  several  sources  for  losses  in  a  motor,  but  as  long  as  the  motor  is 
operated  within  its  prescribed  limits,  most  of  them  may  be  consolidated  into  two  groups; 
constant,  and  directly  proportional  to  velocity,  omega.  Some  exanq>les  are  friction, 
copper  (rR).  and  windage  [McPherson,  1981].  External  loads  may  also  include  constant 
and  velocity  dependent  losses.  The  actual  loss  is  state  dependent  and  requires  some 
fecial  handling: 

(1)  shaft  turning  (omega  <>  0) :  loss  opposes  omega 

l/asjl=Fc  +  FvM;  (4.9) 

(2)  shaft  at  rest  (omega  =  0) :  loss  opposes  torque 

(a)  torque  sufficient  to  overcome  friction  (|torque|  >  Fc) 

loss  =  Fc,  (4.10) 

(b)  torque  insufficient  to  overconw  friction  (requires  torque  -  loss  =  0) 

loss  =  torque .  (4. 1 1 ) 

State  2a  may  be  handled  by  the  method  used  for  state  1,  but  since  it  is  already  detected  by 

the  test  for  state  2b,  there  is  no  reason  to  perform  the  arithmetic.  Incorporating  losses 

into  Equation  4.8  yields 


•  x^,-Loss*s 

j7^T- 


(4.12) 
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The  only  other  significant  loss  not  fitting  into  one  of  the  groups  above  is  the  voltage  drop 
across  the  motor  brushes,  brush  drop  loss  The  vohage  applied  to  the  motor  armature, 
is  the  motor  source  vohage,  minus  this  brush  drop  loss  [McPherson,  1981], 

(4.13) 

Combining  Equations  4.7,  4. 12  and  4. 13,  motor  re^onse  depends  on  construction,  state, 
applied  voltage,  and  load  (external  torque  and  inertia). 


o  = 


'-Losses 


Jm  + 


(4.14) 


As  a  final  note,  motors  are  given  vohage  and  load  ratings.  These  are  determined  by 
motor  construction  and  are  intended  to  keep  armature  current  within  safe  operating  limhs 
[McPherson,  1981]. 


C.  REDUCTION  GEAR 

A  reduction  gear  is  a  mechanical  coupling  device  that  provides  a  mechanical 
advantage  allowing  a  higher  ^eed,  lower  torque  source  to  drive  a  lower  speed,  heavier 
load.  This  enables  both  source  and  load  to  operate  at  or  near  their  optimal,  most  efficient 
speeds,  even  though  those  ^eeds  are  not  the  same.  The  gear  ratio,  n,  is  the  ratio  of  the 
input  and  output  shaft  angular  displacements,  velocities  and  accelerations,  theta,  omega 
and  omega-dot,  re^ectively  [Chen,  1993]. 

0o«,  =  ^,  (4.15) 

=  (4.16) 

(4.17) 
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While  the  output  shaft  speed  is  reduced  by  a  &ctor  of  n,  the  torque  is  mcreased  by  a  fiictor 
of  w  [Chen,  1993]. 


=  (4.18) 

Recalling  and  rearranging  Equation  4.8  and  applying  it  to  the  output  shaft 

Jou,  =  ^.  (4.19) 


Substituting  for  omega-dot and  tau^^  using  Equations  4. 17  and  4. 18 


ns,„ 

(«„/»') 

(4.20) 

(4.21) 

=  Jin 

(4.22) 

This  gives  a  coupling  &ctor  for  inertia  of  Le. 

^  =  (4.23) 

''m 


Combining  a  motor  and  a  reduction  gear  as  a  drive  train  for  some  load.  Equations  4. 12, 
4. 18  and  4.22,  yields: 

(4.24) 


Trf  +  (T^/fi)  -  Losses 

Wn,  —  ~  ,  or 


or 


♦  (w  T^)  +  -  («  LoSMi) 

n*)  +A 


(4.25) 


where  /n  is  the  input  (motor)  side  of  the  reduction  gear,  and  x  is  the  output  side. 
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D.  JOINT  ACTUATOR  SIMULATION 


Figure  4.1  is  a  block  diagram  of  the  Joint  Actuator  Simulation  Model.  Refer  to 
Figure  3.3  for  a  comparison  to  the  actual  Aquarobot  Joint  Actuator.  Since  the  model  was 
in  essence  a  "bench  top"  device,  physical  limitations,  such  as  pulse  counter  overflow  and 
motion  limits,  were  not  detected  or  enforced.  Also,  input  pulses  were  replaced  by  a 
change  in  desired  angular  position  of  the  joint  shaft  in  revolutions,  deha-theta,  and  the 
motor  driver's  rectangular  wav  e  output  is  amplified  to  its  average  value.  Loading  and 
operating  instructions  and  a  complete  source  code  listing  for  this  model  may  be  found  in 
Appendix  A. 


The  D/A  converter  is  not  necessary  in  the  amulation  model  as  the  Pulse  Counter 
stores  the  difference  between  the  current  and  ordered  shaft  positions  rather  than  a  discrete 
pulse  count.  Motor  velocity  and  armature  current,  dots  values  in  the  motor  class,  are 


directly  accessed  to  provide  those  inputs  to  the  driver.  The  cascaded  amplifiers  in  the 
driver  are  separated  and  then  summed  in  order  to  simplify  gain  adjustnmnts.  Positional 
feedback  is  taken  after  the  reduction  gear  to  match  the  inq)lementation  of  the  positioning 
orders  input.  This  is  in  contrast  to  the  physical  device  in  which  feedback  pulses  come 
directly  fi-om  the  motor  hsetf 

1.  Base  Classes 

Several  base  classes  are  used  in  the  joint  actuator  model  Refer  to  Appendix  A 
for  a  source  code  listing.  The  first  is  the  "diff-counter”  class  used  to  model  the  pulse 
counter.  A  single  slot,  "current-count”,  which  is  initialized  to  zero,  holds  the  cumulative 
sum  of  all  deka  theta  orders  and  feedback.  Its  single  method,  ”di£f-signal”,  updates  and 
returns  the  current  count. 

count  =  count + order  -  feedbac  (4. 26) 

The  "amplifier-clipper"  class  has  three  slots:  "anq>lification-fiictor"  (or  gain), 
"max-value"  and  "min-value".  The  "amplify”  method  simply  multiplies  the  argument  by 
the  gain  and  returns  the  product,  clipped  of  course,  to  the  maximum  or  minimum  value  if 
the  product  exceeds  those  prescribed  Hmits. 

The  "driver-class",  a  sub-class  of  amplifier-clipper,  adds  three  gain  slots  for 
independent  anq)lification  of  the  three  ii4)uts  befi>re  summing  them  in  the  final 
amplifier-clipper  stage.  The  current  feedback  is  not  internal  but  relies  on  a  saved  slot 
value  in  the  motor-class. 
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The  "shaft”  class  is  used  as  a  superclass  for  the  motor  class,  as  slots  for  reduction 
gear  input  and  output  shafts,  and  for  the  actual  joint  shaft.  Slots  angular-position  (theta), 
angular-velocity  (omega),  inertia,  coulomb-ftiction-constant  (Fc  or  constant  loss 
component),  viscous-ftiction-constant  (Fv  or  velocity  dependent  loss  conqronent),  and 
time-stamp  are  defined.  Methods  are  defined  to  provide  capability  to  set  theta  and  omega 
to  some  position  and  speed,  reset  theta  and  omega  to  zero,  and  couple  (transfer)  theta  and 
omega  to  another  shaft. 

The  "motor"  class  inherits  fi’om  the  shaft  class  and  defines  additional  slots; 
torque-constant  (A!,),  back-emf-constant  (AT^),  armature-resistance  (RJ,  armature-current 
{ij,  and  brush  drop  parameters  {max-brush-<A'<^  and  half-brush-drop-source^value). 
Methods  "developed-torque"  and  "omega-dot”  are  direct  ing>iementations  of  Equations 
4.7  and  4.14,  re^ectrvely.  Brush  drop  was  approximated  by  using  an  e?q)onential  form 
that  approaches  max-brush-drop  as  the  source  voltage  increases  [McPherson,  1981]. 

Fw  =  Fmo-x  (1-0.5  (4.27) 

Method  "run-motor"  gets  the  elapsed  time  (dt)^  calculates  omega-dot,  then  updates  the 
motor  state  using  Euler  integrations. 

The  "reduction-gear"  class  has  slots  "gear-ratio”  (/i),  "in-shaft"  and  "out-shaft", 
the  last  two  being  instances  of  class  shaft.  Methods  are  provided  to  nnihiply  or  divide  an 
argument  by  n  (n  squared  if  the  argument  is  inertia).  The  method  ”rg-connect"  is  provided 
to  replace  the  shaft  couple  method  for  internal  coupling  and  reduces  the  coupled  theta  and 
omega  by  the  gear  ratio. 
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2.  Joint  Clan 


The  "jornf*  class  actually  rqiresaits  a  joint-actuator.  It  includes  the  entire 
system:  motor  controller,  servo-motor  (prime-mover),  and  reduction-gear.  An  additional 
shaft  slot,  "load-shaft",  is  actually  only  included  to  allow  a  cmivenient  method  to  store  the 
previous  theta  value  in  order  to  determine  the  feedback  delta  theta.  The  output  shaft  of 
the  reduction  gear  actually  holds  the  remaining  load  shaft  slot  values.  The  motor  could 
likewise  have  been  the  reduction  gear  k^ut  shaft;  however,  they  were  kept  separate  for 
clarity. 

Methods  are  provided  to  sum  system  parameters  external  to  the  motor  (i.e. 
load-inertia,  ...)  so  they  may  be  passe<i  into  calls  to  run-motor.  "Feedback”  returns  the 
difference  between  the  output  shaft  of  the  reduction  gear  and  the  load  shaft. 
"Increment-joint"  calls  "run-motor"  then  cotq)les  all  the  shafts,  being  careful  to  save  the 
old  reductirm  gear  output  shaft  position  first  for  use  in  next  call  to  feedback. 
"Step-input-to-joint"  provides  the  fiicility  to  send  delta  theta  orders  to  the  pulse  counter, 
and  "reset-joint"  reinitializes  the  system. 

Loading  "joint.instance.cr  creates  the  instance  of  a  joint  used  for  model  testing. 
The  functions  provided  allow  various  orders  to  be  sent  to  the  joint  and  then  displays 
system  response  to  those  orders.  "Move-joint-muh"  orders  muhiple  repetitions  of  the 
same  "delta-theta"  order,  eadi  order  being  initiated  iq>on  coQq)letion  of  the  previous 
order.  "Move-joint-list"  is  gmilar  in  execution,  but  takes  a  list  (sequoice)  of  deka-theta's 
rather  than  repeating  the  same  one.  "Run-joint"  orders  a  continuous  sequence  of  small 
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deka-thetas,  determined  by  argument  "speed”  and  system  elapsed  time,  required  to  achieve 
the  ordered  RPM  speed.  The  remaining  functions  are  intended  to  be  internal  calls.  They 
are  "clear-and-reset"  (called  by  each  of  the  previous  three  for  initialization), 
"move-joint-list<2"  (recursive  call  for  move-joint-list),  and  "move-joint"  (called  by 
move-joint-mult  and  move-joint-list-2).  "Move-joint"  is  the  workhorse  and  makes 
repeated  calls  to  method  "increment-joint”  until  the  ordered  deha-theta  has  been  achieved 
(the  pulse  counter's  current  count  approaches  zero).  In  contrast,  "nm-joint”  makes 
repeated  calls  to  "increment-joint"  until  the  ordered  ^eed  is  achieved. 

3.  Additional  Supporting  Code 

Loading  "Window,  instance,  cl"  creates  a  display  window  with  a  gradicule.  A  call 
to  "display-state"  reads  the  appropriate  state  slots  and  draws  the  set  of  data  points  for  the 
current  time.  Additional  methods  are  provided  for  internal  calls  and  for  reinitialization 
(i.e.  clearing  or  resetting  the  window).  The  end  resuh  is  a  di^lay  of  system  state  vs.  time 
for  program  output.  Finally,  the  file  "joint-loader"  is  provided  as  a  convenience,  and 
loading  it  ensures  loading  of  the  source  code  files  in  the  correct  sequence  (dependencies 
are  observed).  After  loading  the  source  code,  it  then  makes  calls  that  test  run  various 
features  of  the  system. 

E.  SUMMARY 

This  chapter  provides  a  review  of  the  baric  mathematics  required  to  model 
servomotors  and  reduction  gears  and  then  describes  the  simulation  of  an  Aquarobot  joint 


29 


actuator.  The  noodel  is  amplified  in  that  exception  handling  control  signals  are  ignored 
and  the  pulse  counter  input  is  desired  deha-theta  for  the  joint  shaft,  but  fimoionally,  it  is 
equivalent.  In  the  next  chapter,  the  Aquarobot  model  is  presetted;  howevw.  joint 
actuators  are  not  yet  included  but  instead  are  fimctionally  represmted  by  springs  and 
dampers. 
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V.  SPRING  AND  DAMPER  MODEL 


A.  INTRODUCTION 

A  modified  dynamic  simulation  model  for  Aquarobot  is  developed  in  this  chapter.  It 
has  springs  and  dampers  in  place  of  servo  motors  to  provide  joint  torques.  The  springs 
and  danqjers  are  intended  to  eventually  be  replaced  by  a  joint  actuator  model  such  as  that 
presented  in  the  previous  chapter.  The  model  can  be  tested  by  giving  the  Aquarobot  an 
initial  position  and  orientation  and  then  allowing  it  to  drop,  observing  the  response  as  the 
feet  contact  the  ground  and  the  legs  provide  support. 

Since  the  purpose  of  the  model  is  to  provide  an  approximate  dynamic  simulation, 
capable  of  running  in  real  time,  several  simplifications  have  been  made: 

(1)  the  legs  are  considered  to  be  massless; 

(2)  the  ground-foot  fiiction  is  infinite  (no  ^page); 

(3)  the  center  of  mass  is  assumed  to  be  at  the  geometric 
center  of  the  inboard  joints  of  the  legs;  and 

(4)  body  inertia  is  that  of  a  solid  homogeneous  cylinder. 

In  addition,  joint  stops  (physical  limits)  in  the  kinematic  model  are  disabled  and  collisions, 
other  than  feet  contacting  the  ground,  have  been  ignored. 
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R  INVERSE  KINEMATICS 


The  Aquarobot  leg  kmematic  model  allows  detennmation  of  foot  poshion,  given  the 
joint  angles.  The  inverse  kinematic  problem,  on  the  other  hand,  is  to  determine  the  joint 
angles,  given  the  foot  position.  Using  the  appropriate  coordinate  systems  simplifies 
inverse  kinematics.  Figure  S.  1  illustrates  the  coordinate  system  used  to  calculate  joint  I's 
angle,  theta  I ;  the  joint  is  the  origin,  the  Z-axis  is  down  (parallel  to  body  Z-axis)  and  the 
X-axis  is  radially  outward  fi-om  the  body.  Theta  1  is  measured  as  a  right  handed  Z-axis 
rotation  using  the  X-axis  as  a  zero  reference. 


Top  View  of  Joint  1  Coordinate  System  Used  to  Calculate  Theta  1. 

Given  the  foot  po^on  in  this  coordinate  system,  theta  1  is  easily  calculated: 

01  =arctan(g).  (5.1) 
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Figure  5.2 

Side  View  of  Coordinate  System  Used  to  Calculate  Theta2  and  Theta3. 

Figure  5.2  illustrates  the  coordinate  system  used  for  calculating  theta2  and  theta3.  Joint2 
serves  as  the  origin,  the  X-axis  is  defined  as  the  direction  directly  away  fi-om  joint  1 .  and 
the  Z-axis  is  again  down.  Using  this  coordinate  system  reduces  the  problem  to  two 
dimensions  as  the  y-component  of  the  foot  position  is  now  zero.  Figure  5.3  and  Equation 
5.2  illustrate  the  law  of  cosines  [Oakley,  1971]  t^ch  is  used  to  determine  theta2  and 
theta3. 

-  2bc  cos  (5.2) 


joints  ^hetaS 

‘‘Z  \ 

/  \  jointZ  y 

'^+theta2\*-^  X 

<^B  \b  / 
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c  \  z(foot) 

\a\ 

V 

D  Vv 

z< 

^  foot 

x(foot) 

Figure  5.3 

Applying  Law  of  Cosines 
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Solving  Equation  S  .2  for  angle  A; 


/I  =arccos[^^^^].  (5.3) 

Angles  B  and  C  are  determined  in  the  same  manner; 

S  =  arccos[=^— ].  (5.4) 

C  =  arccos[:^].  (5.5) 

Referring  back  to  Figure  5.3,  sides  a  and  b  correspond  directly  to  the  lengths  of  Iink2  and 
links,  respectively,  and  side  c  may  be  calculated  u^g  Pythagorean's  theorem: 

^  ~  J^fool  -/ool  ■  (5.6) 

Angle  D  may  be  determined  by  several  trigonbmic  hmctions;  arcsin  is  used  here: 

D  =  arcsin[^].  (5.7) 

Ihetal  and  thetaS  are  measured  u^g  the  agn  convention  shown.  Note  that  thetaS  is  the 
negative  of  C's  compliment: 

02=fl-D,  (5.8) 

and 

03  =  C-7i.  (5.9) 

Combining  Equations,  (5.4,  5.7  and  5.8)  and  (5.5  and  5.9),  and  substituting  L2  and  L3  for 


a  and  b  resuhs  in 

02  =  arccos 

-  arcsin [^],  and 

(5.10) 

1 — 

1 _ 

03  =  arccos 

ij  +lI-c^ 

-71. 

(5.11) 

2LiLi 

C.  JACOBIAN  MATRIX 


The  Jacobian  Matrix,  J^i)  or  simply  of  vector  r  with  respect  to  vector  q  is 


defined; 


(5.12) 


and  is  used  to  express  the  relation  between  an  end  effector  velocity  and  the  joint  velocities 
of  a  manipulator  [Yoshikawa,  1990],  In  the  case  of  Aquarobot,  the  foot  velocity  with 
respect  to  leg  joint  velocities  is  given  by; 


foot  0  1 

•  • 

y/oot  ~  02 


(5.13) 


By  rearranging  Equation  5.13,  the  inverse  Jacobian  and  the  foot  velocity  may  be  used  to 
determine  the  joint  velocities; 


02  “  yfixA  y/oi 


(5.14) 


The  Jacobian  will  also  be  used  in  the  next  section  to  determine  ground  reaction  forces. 

The  derivation  of  the  Jacobian  matrix  for  an  Aquarobot  leg  is  straightforward.  The 
foot  position  of  an  Aquarobot  leg  is  kinematically  described  as  a  fimction  of  the  joint 
angles  in  [Schue,  1993]: 


X  =  IoCOS0o+Z-lCOS0oi  +Z,2COS0oiCOS02 +£.3COS0oiCOS02  , 


(5.15) 


y  =  Z,osin0o  +Iisin0oi  +I2sin0oicos02  +I.3rin0oicos023, 


(5.16) 


r  =  -  Z.2sin02  -I3sin023, 


(5.17) 


A^ere  is  linkj  length,  theta^  is  joints  angle,  theta^  is  the  sum  of  thetaj  and  thetaj,  and  link^ 
represents  the  constant  pseudo-link  from  center  of  body  to  joint,  and  has  a  joint  angle 
equal  to  the  "leg  attachment  angle".  Differentiating  Equations  S.  IS  through  S.  17  gives; 

X  =  -Li^sinOoi  8i  ^  -L2(sm0oicos02  0i  +cos0oisin02  02  ) 

-£3(^sm0oicos023  01  +cos0oisin023(02 -^3  ))  .  (5.18) 

V  = -Z,i(^cos0oi  01  )  -I2(cos0oicos02  01  +  sin0oi sin02  02  ) 

-A 3 (cos 001  cos 023  01  +sin0oisin023(02  •^^3  ))  ,  (5.19) 

=  =  -Z.2(cOS02  02  )  -Z.3(cOS023(02  +03  ))  •  (5.20) 

Regrouping  and  translating  Equations  S.  18  through  5.20  into  the  form  of  Equation  5. 13, 
the  Jacobian  Matrix  is  given  by 

-(ii +  i2cos02 +ijcos023)sin0oi  -(Z.2Sin92-i3sin023)cos0oi  -Z,3cos0oisin023 
J=  (ii +£,2Cos02+Z,jcos023)cos0oi  -(Z,2sin02 - Z,3sin023)sin0oi  -£3Sin0oisin023 
0  -Z,2COS02-Z,3COS023  -Z,3COS023 

(5.21) 


D.  FORCES  ON  AQUAROBOT 

Assuming  homogeneous  c)dindrical  distribution  of  body  mass  and  massless  legs 
reduces  the  con:q)lexity  of  the  forces  and  torques  on  Aquarobot.  As  Figure  5.4  illustrates, 
the  resulting  summations  of  these  forces  and  torques  may  be  expressed 

+  A/"*’ 


36 


and 


T  Aquarobot 


—  ^  f’ltg 
leg^X 


(5.23) 


where  is  ground  reaction  force,  and  is  the  mom»t  arm,  from  the  body's  center  of 
mass  to  the  foot,  on  which  that  force  is  exerted. 


Given  an  initial  known  state  (position,  orientation  and  velocity),  Aquarobot’s  motion  may 
be  completely  described  by  application  of  Newton's  second  law.  The  kinematic  solution 
for  is  already  available  [Davidson,  1993],  so  all  that  remains  is  determination  of  f,^^. 

The  torques  at  the  joints  in  a  manipulator  are  related  to  the  force  exerted  by  the  end 
effector  by  the  transpose  of  the  Jacobian  [Yoshikawa,  1990]: 


T= 


Te, 


(5.24) 
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wfaere/is  the  force  the  foot  exerts  on  the  ^ound,  equal  magnitude  but  opposite  direction 
of  ground  reaction  force.  To  avoid  confusion,  /  will  thus  refer  to  the  ground  reaction 
force,  the  force  the  ground  exerts  on  the  foot,  as  it  is  the  force  in  which  we  are  interested; 
therefore  Equation  5.24  must  have  a  agn  change: 

(5.25) 

Solving  Equation  5.25  for /  yields; 

7  =  (5.26) 

E.  SPRING  AND  DAMPER  JOINT  TORQUES 

In  the  ^ring  and  damper  model,  joint  torque  is  the  sum  of  spring  restoring  torque  and 
damping  torque; 

Xjoin,  =(-ks(.Q  -  0o))  +  (-kd  e)  ,  (5.27) 

where  and  kj  are  ^ring  and  daiiq>ing  constants,  and  thetaO  is  the  ordered  position.  For 
the  remainder  of  this  chapter,  thetaO  will  be  taken  as  the  rest  position  or  zero  torque 
reference.  For  convenience,  the  symbols  on  the  right  ade  of  Equation  5.27  will  be  used  to 
refer  to  their  vector  forms  and  will  represent  all  three  joints  in  a  leg.  Replacing  the  left 
side  with  T,  and  assuming  the  same  ^ring  and  damping  constants  for  each  joint, 

T  e,  1  r  01,  T|  r  0,  ' 

T=-ks  02  -  02,  02  .  (5.28) 

03  .  _  03,  j  03 
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The  joint  angles  are  determined  using  the  inverse  kinematic  method  described  above 
and  are  then  available  for  the  Jacobian  matrix.  For  any  foot  in  contact  with  the  ground,  its 
velocity,  relative  to  the  body,  is  simply  the  negative  of  the  sum  of  the  body's  translational 
velocity  and  the  cross  product  of  the  body’s  rotational  velocity  with  the  foot's  position 
vector.  Combining  Equations  5. 14  and  5.27,  and  substituting  for  foot  velocity  gives  us; 

7'=-*^  (e-0o)  +  A:rfV’'  [Vbo<iy+{!^bo<h  ^rfooi)].,  (5.29) 

which  is  now  substituted  into  Equation  5.26  to  give  us  the  ground  reaction  force: 

/  =  ~ ®o)  - kdJ-^ [Vbody  +  (dboJy  X  rfoot)] .  (5.30) 


F.  LISP  PROTOTYPE 

The  kinematic  model  for  the  Aquarobot  simulation  was  borrowed  from  [Davidson, 
1993]  with  the  only  code  modification  being  the  conversion  to  Modified 
Oanevh-Hartenberg  (MDH)  coordinate  systems  to  match  the  C++  version.  The  complete 
source  code  listing  with  loading  and  operating  instructions  may  be  found  in  Appendix  C. 
The  additions  required  for  dynamic  operation  of  the  model  are  best  presented  by  the 
following  walk-through  of  a  dynamic  update.  Figure  5.5  provides  a  flowchart  for 
reference. 

A  dynamic  update  of  Aquarobot  is  achieved  by  calling  method  "update-aquarobot”. 
The  body's  acceleration,  velocity  and  position  are  all  updated  by  calls  to  previously  defined 
methods  in  the  rigid-body  class.  After  these  are  conq)leted,  and  the  body  is  repositioned, 
the  legs,  forces  and  torques  are  then  updated  by  calling  "update-forces-and-torques". 
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Figure  5.5 

Flowchart  for  Dynamic  Update  of  Aquarobot. 


Gravity  is  handled  sq>aratety  in  the  "update-acceleration''  method;  therefore,  only  the 
forces  and  torques  due  to  foot  contact  with  the  ground  need  be  considered. 
Update-forces-and-torques  resets  the  body's  forces-and-torques  vector,  a  rigid-body  slot, 
to  zeros,  then  calls  "add-leg-forces-and-torques"  for  each  leg  to  generate  an  iq>dated 
cumulative  value. 
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Add-leg-forces-and-torques  tests  for  “foot-contact”,  a  Boolean  type  slot,  or 
"new-contact",  a  fiinction  that  sets  foot-contact  to  true  and  the  foot's  world  z  coordinate 
to  zero  (ground  level)  if  ground  penetration  is  detected  (z  coordinate  greater  than  zero). 
If  there  is  no  foot  contact,  nothing  happens:  joint  angles  remain  set  to  their  de&uh  values 
and  the  body's  cumulative  forces-and-torques  value  is  left  unchanged.  If  however,  there  is 
contact,  the  inverse  kinematics  routine  is  caUed,  the  ground  reaction  force  is  calculated 
using  Fquation  S.30,  and  the  joint  angles  are  updated.  Before  updating  the  cumulative 
force ,  a-torques,  loss  of  contact  must  be  detected.  This  is  done  by  testing  the  world  z 
component  of  the  calculated  ground  reaction  force  in  a  call  to  "still-in-contact".  If  the 
force  is  such  that  it  is  pulling  the  robot  down  rather  than  supporting  it,  then  foot-contact  is 
set  to  fidse,  the  joint  angles  are  reset  to  their  defimh  values,  and  again,  no  contribution  to 
forces-and-torques.  If  the  foot  is  determined  to  be  still-in-contact,  world  z  conq)onent  of 
the  force  less  than  zero  (pushing  up),  then  method  "add-forces-and-torques-to-body"  is 
called  which  adds  /  and  r  x  /  to  the  cumulative  value  of  forces-and-torques  as  in 
Equations  5.22  and  5.23.  After  cycling  throu^  all  the  legs,  Aquarobot  is  completely 
updated  and  ready  for  another  cycle. 

This  dynamic  update  cycle  actually  uses  the  (/  +1)^  velocity  for  the  update.  While 
the  velocity  might  just  as  easily  have  been  used,  h  is  neither  better  nor  worse.  Better  is 
actually  the  average  of  the  two. 
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G.  C-H-  PROTOTYPE 


The  C++  version  of  the  dynamic  Aquarobot  OMMlel  is  algorkhmicaUy  identical  to  the 
LISP  version.  A  complete  source  code  listing  with  operating  instructions  may  be  found  in 
Appendix  D.  As  discussed  in  Chapter  III,  use  of  QUS  Performer  structures  required  some 
modifications.  For  example,  six-vectors  were  divided  into  pairs  of  three-vectors.  The 
dynamic  model  is  otherwise  identical  A  feature  added  to  this  version  is  the  ability  to  pass 
spring  and  danq>er  constants,  drop  height,  and  dynamic  update  time  increment  into  main 
as  command  line  arguments.  After  handling  these  optional  arguments,  main  initializes 
Performer,  instantiates  and  inhiaUzes  dynamic  and  graphic  Aquarobot  objects,  then  cycles 
in  an  update-render  loop. 

The  "graphic  Aquarobot"  object  was  not  required  in  the  LISP  verson  as  the  "dynamic 
Aquarobot"  slot  values  were  directly  accessed  to  render  a  stick  figure.  This  version, 
however,  draws  thousands  of  fifled  po^gons  eadi  cycle  to  render  a  single  firame.  DUS 
Performer  was  used,  as  previously  stated,  primarily  for  its  high  performance  in  this  ta^. 
The  graphic  Aquarobot  is  a  hierarchical  database  containing  the  information  required  to 
draw  Aquarobot.  After  each  dynamic  iq>date,  the  body's  position  and  orientation  and  the 
leg's  joint  angles  are  pa&^  into  "Dynamic  Coordinate  Systems”  in  the  database  prior  to 
calling  the  draw  routine. 
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H.  SUMMARY 


This  chapter  develops  a  siiiiplified  dynamic  simulation  model  for  Aquarobot  using 
springs  and  dampers  to  provide  the  joint  torques.  Inverse  kinematics,  Jacr^ian  matrices 
and  their  utility,  and  forces  acting  on  Aquarobot  are  discussed.  The  model  was 
implemented  and  tested  using  both  LISP  and  C++.  In  the  next  chapter,  simulation  resuhs 
of  the  spring  and  damper  model,  and  also  of  the  joint  actuator  model  from  Chapter  IV.  are 
presented. 
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VI.  SIMULATION  RESULTS 


A.  INTRODUCTION 

This  chapter  presents  the  simulation  results.  The  joint  actuator  simulation  model  was 
tested  to  verify  the  model  and  to  experimentally  determine  suitable  amplifier  gains.  As 
stated  in  the  previous  clu^)ter,  the  spring  and  dan^)er  Aquarobot  modd  was  tested  by 
dropping  the  robot  fi’om  a  low  hdght  and  observing  is  dynamic  bduvior. 

B.  JOINT  ACTUATOR  SIMULATION 

The  motor  class  is  designed  such  that  constructor  arguments  are  parameters  listed  on 
the  motor  specification  sheet.  At  the  time  of  the  simulation,  specification  sheets  for  the 
motors  used  in  Aquarobot  were  not  available;  therefore,  another  modd  was  used.  The 
model  was  tested  by  applying  rated  voltage  to  the  motor  and  observing  its  acceleration 
and  attained  RPM.  In  Figure  6. 1,  both  motor  and  output  shaft,  after  200: 1  reduction  gear, 
speeds  and  positions  are  shown.  Motor  scales  are  on  the  left,  while  output  shaft  scales  are 
on  the  right.  Qualitatively,  the  motor  model  is  well  behaved,  and  quantitativdy,  it  closely 
matches  the  no-load  speed  parameter  listed  in  the  specifications. 
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Figure  6.1 

No  Load  Joint  Actuator  Response  >^tth  +7SVDC  ^plied  to  the  Motor 


To  obtain  ^  joint  actuator  response  to  input  orders,  it  is  desirable  to  set  the  error 
signal,  D/A  converter  output,  gain  to  a  relatively  hi^  value.  A  gain  of  ISO  resulted  in  the 
optimum  response.  Values  from  100  to  200  gave  satis&ctoiy  results  while  higher  values 
progressively  reduced  the  effectiveness  of  the  degenerative  feedback.  Figures  6.2  through 
6.5  show  joint  actuator  responses  to  the  foUowing  sequence  of  shaft  positioning  orders,  in 
revolutions,  with  various  driver  gain  values;  (+1/4,  +1/2,  -1,  -1/2,  +1,  +3).  Figure  6.2 
illustrates  the  response  with  an  error  signal  gain  of  150  and  with  velocity  and  current 
feedback  signals  disabled. 
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Figure  6.2 

Joint  Actuator  Response  with  Error  Signal  Gain  of  1  SO. 
Velocity  and  Current  Feedback  Disabled. 


The  optimum  velocity  feedback  signal  gain  is  a  compromise  between  response  time 
and  stability.  Too  high  a  value  results  in  slower  response,  whUe  lower  values  allow 
increased  overshoot.  Values  from  three  to  seven  were  satisfrctoiy.  A  gain  of  five  proved 
optimum  when  combined  vdth  current  feedback.  Figure  6.3  displays  the  results  of  using 
various  gain  values  for  velocity  feedback  with  current  feedback  still  disabled. 
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Figure  6.3a 

Joint  Actuator  Response  with  Velocity  Feedback  Gain  of  3 
and  Error  Signal  Gain  of  ISO.  Current  Feedback  Disabled. 


Figure  6.3b 


Joint  Actuator  Response  with  Velocity  Feedback  Gain  of  5 


and  Error  Signal  Gain  of  ISO.  Current  Feedback  Disabled. 
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Figure  6.3c 

Joint  Actuator  Response  with  Velocity  Feedback  Gain  of  7 
and  Error  Signal  Gain  of  ISO.  Current  Feedback  Disabled. 
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Figure  6.3d 

Joint  Actuator  Response  with  Velocity  Feedback  Gain  of  20 
and  Error  Signal  Gain  of  1 50.  Current  Feedback  Disabled. 
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The  current  feedback  gain  value  turned  out  to  be  the  most  sensitive.  A  value  of  0.3 
produced  negligible  change,  while  0.7  resulted  in  some  oscillation.  This  regenerative 
feedback,  intended  to  reduce  response  time,  also  aided  in  reducing  the  overshoot.  As  the 
error  signal  approached  zero  with  speed  on,  back  EMF  caused  a  decelerating  current 
which  was  aided  by  the  regenerative  current  feedback.  Figure  6.4  displays  the  results  of 
using  various  gain  values  for  current  feedback  with  velocity  feedback  again  disabled.  In 
addition,  in  the  presence  of  the  degenerative  velocity  feedback,  the  error  signal  was 
overcome  earlier  by  decelerating  signals,  bringing  the  current  feedback  contribution  in 
even  sooner.  Figure  6.5  illustrates  results  with  both  velocity  and  current  feedbacks  in 
effect.  The  response  of  the  joint  actuator  in  this  simulation  highlights  the  effectiveness  of 
the  design;  high  error  signal  gain  with  velocity  and  current  compensating  feedback 
networks. 

While  these  tests  results  are  not  for  the  specific  motors  used  in  Aquarobot,  they 
provdde  a  good  set  of  gtun  parameters  to  use  as  a  starting  point  in  the  experimental 
determination  of  the  gains  for  those  motors. 


Figure  6.4a 

Joint  Actuator  Response  with  Current  Feedback  Gain  of  0.3 
and  Error  Signal  Gain  of  1  SO.  Vdocity  Feedback  Disabled. 


Figure  6.4b 

Joint  Actuator  Response  with  Current  Feedback  Gain  of  0.5 
and  Error  Signal  Gain  of  1  SO.  Velocity  Feedback  Disabled. 
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Figure  6.4c 

Joint  Actuator  Response  with  Current  Feedback  Gain  of  1 .0 
and  Error  Signal  of  1  SO.  Velocity  Feedback  Disabled. 
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Figure  6.Sb 

Joint  Actuator  Response  with  Velocity  Feedback  Gain  of  S, 
Error  Signal  Gain  of  1  SO,  and  Current  Feedback  Gain  of  O.S. 
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Figure  6.  Sc 

Joint  Actuator  Response  with  Velocity  Feedback  Gain  of  7, 
Error  Signal  Gain  of  150,  and  Current  Feedback  Gain  of  O.S. 


Figure  6.Sd 

Joint  Actuator  Response  with  Velocity  Feedback  Gain  of  10, 
Error  Signal  Gain  of  150,  and  Curr«it  Feedback  Gain  of  0.5. 
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C.  AQUAROBOT  SPRING  AND  DAMPER  SIMULATION 

LISP  and  C-H-  versions  of  the  q;>ring  and  dan4)er  AquaroIxM  model  were  tested  by 
using  a  "droptest"  in  \^ch  the  model  is  dropped  from  low  height.  It  may  be  tihed,  but 
not  so  much  that  it  does  not  land  on  its  feet.  The  LISP  version  served  as  the  prototype, 
and  after  successful  testing,  the  model  was  translated  into  C-h-. 

The  LISP  simulation  ran  uncompiled  on  a  Sun  Sparc- 10  at  six  to  eight  frames  per 
minute  and  was  too  slow  for  comprehension  of  nx>tion  detail.  It  did,  however,  allow 
experimental  determination  of  spring  and  damper  constants  sufficient  to  support  the  nuxlel 
when  dropped.  To  increase  the  simulation  speed,  two  dyiuunic  update  cycles  were  itm 
between  each  display,  and  the  source  code  was  then  compiled.  After  con^ilation,  the 
simulation  ran  at  near  30  frames  per  minute,  with  60  dynamic  updates  of  SOms  each,  to 
achieve  a  simulation  with  i^rproximatdy  a  10:1  time  dilation.  This  simulation  was  &st 
enough  for  an  observer  to  assimilate  the  dynamic  behavior  of  the  model  which  was 
qualitatively  satis&ctory  and  considered  successfiil.  Some  additional  fine  tuning  of 
experimentally  determined  parameters  was  done  prior  to  translation  to  C-h-. 

After  translation  to  C-H-/perfr)rmer,  the  modd  was  again  tested,  and  a  real-time 
simulation  was  achieved  on  a  four  processor  IRIS  440/RE  workstation.  Only  three 
processors  were  actually  utilized,  one  assigned  to  each  of  the  following  tasks:  application, 
database  pre-draw  cull,  and  database  rendering  traversal.  The  application  processor 
utilization  was  iq)proxirtMtely  fifteen  percent  which  indicates  that  the  increased  complexity 
of  adding  the  joint  actuators  will  not  present  any  difficulty.  The  cull  processor  utilization 
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was  apf^cndmittely  twenty  percent  while  that  of  the  rendering  ivocessor  veried  from  fifty 
to  sevemy  percent.  (Note;  the  source  code  was  not  conqnled  and/or  linked  with 
optimizations  on.)  Typical  images  obtained  can  be  fixind  in  [Goetz,  1994]. 

Running  in  the  three  processor  configuration  described  above,  Perfom^ 
synchronized  the  fiemerate  to  the  fixed  SOms  dynamic  updates  by  limiting  the  fiemerate  to 
20Hz.  On  a  single  processor  IRIS  R-4000,  wliere  the  application,  database  cull,  and 
database  rendering  were  forced  to  nm  sequentially,  the  hi^iest  fiemerate  achieved  was 
lOHz.  This  resulted  in  a  2;  1  time  dilation  (slow  down)  simulation. 

D.  SUMMARY 

Testing  of  both  simulation  models  was  considered  successful.  While  it  is  unfortunate 
that  there  was  insufficient  time  to  incorporate  the  joint  actuator  modd  into  the  Aquard>ot 
model,  replacing  the  springs  and  dampers,  tlw  topic  was  given  some  time  and  effort.  This 
next  step  is  among  the  topics  addressed  in  the  final  chapter:  Summary  and  Conclusions. 
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VII.  SUMMARY  AND  CONCLUSIONS 

A.  UTILITY  OF  LISP  FOR  EXPERIMENTAL  PROGRAMMING 

The  primary  ben^t  of  using  LISP  as  a  prototyping  language  in  this  theas  was  the 

immediate  testing  capability  it  provided.  No  test  routines  were  required.  Each  function 
was  easily  tested  by  direct  calls  as  it  was  devdoped.  While  compilation  cq>abiiity  allowed 
a  faster  simulation,  repeated  compilations  woe  not  required  as  the  routines  could  caUed 
from  the  interpreter's  command  line.  Finally,  nesting  ftinctions  allowed  larger  and  larger 
integrated  blocks  of  source  code  to  be  tested. 

One  of  the  benefits  of  uang  LISP  during  prototyping  was  realized  \dien  an  ^parent 
limit  cycle  appeared  if  Aquarobot  was  tilted  when  dropped.  The  problem  was  actually  a 
lack  of  rotational  damping  due  to  the  absence  of  the  omega  cross  r  term  in  the  foot 
velocity  calculation.  \S^thout  it,  damping  ceased  shortly  afta-  landing  because  of  "zero 
translational  foot  velocity."  While  the  author  took  a  considerable  period  of  time  to  find 
the  cause  of  the  problem,  with  a  single  modification  to  the  LISP  source  code,  the 
correction  was  quickly  and  easily  verified. 

B.  INCORPORATING  THE  JOINT  ACTUATOR  MODEL 

The  next  step  required  for  the  Aquarobot  dynamic  simulation  model  is  to  r^lace  the 
springs  and  dampers  with  joint  actuators.  This  was  initially  assumed  to  be  a  simpler  task 
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than  it  turned  out  to  be.  The  difficulty  arises  from  a  conflict  ova*  control  of  joint  state 
variables.  In  the  spring  and  damper  model,  the  joints  only  supply  state  depoident  torques 
which  are  then  used  to  dynamically  update  the  robot's  body.  After  the  body  is  updated, 
the  new  states  of  the  joints  depend  only  on  the  new  body  position  and  not  on  the  joint 
torques.  In  the  joint  actuator  model,  the  joint  state  depends  directly  upon  the  motor 
torque.  Two  possible  solutions  are  proposed  to  eliminate  the  conflict. 

One  possible  solution  is  to  extend  the  massless  leg  simplification  to  the  motor  and 
gear-train,  making  them  inertialess.  While  this  seems  easiest  and  will  probably  have  as 
little  impact  as  the  massless  legs,  the  overall  effect  may  be  greater  than  anticipated  due  to 
the  large  reduction  ratios  involved.  Recall  that  motor  inertia  reflected  outside  the 
reduction  gear  is  multiplied  by  the  square  of  the  reduction  ratio.  A  C++  version  of  the 
joint  actuator  model  is  included  in  Appendix  B.  This  model  is  a  variation  of  the  original 
LISP  version  and  provides  state  dependent  torques  as  output  rather  than  the  state  itself  It 
could  be  used  to  implement  inertialess  joint  actuators,  and  the  changes  required  in  the 
Aquarobot  model  would  be  minor. 

Another  possible  solution  is  to  use  the  concept  of  "added  mass,"  an  apparent  increase 
in  mass  (affecting  acceleration)  due  to  the  internal  inertia  of  the  drive  motors.  Assuming  a 
unit  acceleration  for  one  of  the  body's  sue  degrea  of  freedom,  it  is  posable  to  calculate  the 
resulting  joint  accelerations.  If  a  joint  acceleration  is  known,  inertial  torque  in  the  joint 
can  be  calculated.  The  net  torque  for  the  joint  then  is  armature  torque  (determined  using 
motor  applied  voltage  and  speed)  minus  the  inertial  torque.  Doing  this  for  all  six  degrees 
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of  freedom  ^ves  a  matrix  of  added  mass  which  can  be  inverted  to  get  accdoation  for  any 
given  vector  of  joint  motor  applied  vohages.  An  "equilibrium  torque,"  torque  vector 
which  results  in  a  zero  joint  acceloation  vector,  is  also  needed  and  must  be  calculated 
[Koozekanani,  1983]. 

C.  SUMMARY 

IRIS  Performer  has  proven  its  utility  as  a  graphics  rendering  tool  in  a  real  time 
simulation.  It  also  provided  easy  synchronization  for  fixed  duration  integration  intervals. 

A  real-time  dynamic  simulation  of  Aquard>ot  was  accomplished  and  is  eventually 
expected  to  provide  a  valuable  tool  for  Aquarobot  control  software  developers.  While  an 
integrated  model,  with  the  joint  actuators  in  place,  is  not  yet  completed,  we  are  a  step 
closer,  and  the  task  has  certainly  proven  to  be  fearible.  Once  the  joint  actuators  are 
installed  into  the  Aquarobot  model,  simple  walking  emulations,  on  smooth,  flat  terrain 
may  be  achieved.  Concurrent  work  on  collision  detection  for  uneven  terrain  will  also 
further  improve  the  simulation  model  when  incorporated  [Goetz,  1994]. 

Further  woric  could  improve  on  the  Performer  Aquarobot  rendering  database  to 
decrease  the  rendering  time.  This  is  the  area  where  there  is  the  most  room  for 
improvement  in  cycle  time.  Finally,  other  than  using  the  "frster"  Performer  routines,  no 
attempt  has  been  made  to  optimize  the  source  code  which  was  written  with  ease  of 
modification  in  mind.  Utilization  of  compile  and  link  optimizations,  and  eventually  some 
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s(Hirce  code  tuning,  may  also  contribute  toward  achievement  of  a  real-time  Aquarobot 
simulation  on  a  sin^e  processor. 
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APPENDIX  A 


LOADING  AND  OPERATING  INSTRUCTIONS 


To  nin  demo,  start  LISP  Interpreter  and  call; 
(load  "joint-loader”) 


SOURCE  CODE  FILES 


;  "joint-loader" 


;  load  files 

(load  "niath.routines.cr) 

(load  "time.routines.cr) 

(load  "diff-counter.class.cr) 

(load  "amplifier<lipper.class.cr) 
(load  "shafl.ciass.cr) 

(load  "motor.class.cr) 

(load  "reduction-gear.class.cr) 
(load  "joint.class.cr) 

(load  "joint.instance.cr) 

(load  "window.instance.cl") 

;  execute  tests 
(move-joint-mult  -.25  4) 
(move-joint-mult  .25  4) 
(move-joint-mult  .05  4) 
(move-joint-list  '(-25  .5-1  -.5  1  3)) 
(run-joint  15) 
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;  "difT-couiiter.class.cr' 


(defclass  diff-counter  () 

{(current-count 

:accessor  current-count 
;initfonn  0 
:t\pe  float ))) 

(defmethod  di£f-signal  ((dc  diff-counter)  plus-input  ntinus-input) 
(self  (current-count  dc) 

(+  (currcnKount  dc)  plus-input  (-  minus-input)))) 


;  "amplifler-clipper.class.cl'' 


(defclass  amplifier-clipper  () 

((amplification-factor 
:initarg  ;amplification-factor 
:  accessor  amplification-factor 
:initform  1 
ity-pe  float) 

(max-value 
:initarg  ;  max-value 
:  accessor  max-value 
:initform  1 
:type  float) 

(min-value 
:initarg  ;  min-value 
;accessor  min-value 
:initform  -1 
:t>pe  float))) 

(defmethod  amplify  ((amp  amplifier-clipper)  input-value) 
(max  (min  (*  (amplification-factor  amp)  input-value) 
(max-value  amp)) 

(min-value  amp))) 
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(defclass  motor-driver  (amplifier-clipper) 

((displacement-gain 
'.initarg  .displacement-gain 
laccessor  displacement-gain 
:initform  1 
;type  float) 

(velocity-fb-gain 
linitarg  :velocit>-fb-gain 
:  accessor  velocity -fb-gain 
;initform  0 
:t>pe  float) 

(current-fb-gain 
:  initarg  ;cunent-fb-gain 
:accessor  current-fb-gain 
:initform  0 
:t>pe  float))) 

(defmethod  drive((driver  motor-driver)  displacement-input 
velocity-input 
cunent-input) 

(amplify’  driver  (+  (•  displacement-input  (displacement-gain  driver)) 
(•  (-  velocity-input)  (velocity-fb-gain  driver)) 

(*  current-input  (current-fb-gain  driver))))) 
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;  "shaft.class.cr' 


(defclass  shaft  () 

((angular-position  ,  radians 

laccessor  theta 
linitform  0 
:t>pe  float) 

(an^ar-veiocit>’  ;  rad/sec 

:accessor  omega 
initform  0 
type  float) 

(inertia  ;  Kg-(meters-square) 

initarg  :I 
accessor  I 
initform  0 
:type  float) 

(coulomb-friction-constant ;  Newton-meters  (Fc  >=  0) 

;  initarg  :Fc 
: accessor  Fc 
:  initform  0 
:type  float) 

(viscous-friction-constant ;  Newton-meters/(rad/sec)  (Fv  >=  0) 
;initarg  :Fv 
:accessor  Fv 
:  initform  0 
:type  float) 

(time-stamp  ;  seconds 

:accessor  time-stamp 
:  initform  (system-time)))) 

(defmethod  set-shaft  ((s  shaft)  theta  omega) 

(setf  (omega  s)  omega) 

(setf  (theta  s)  theta)) 

(defmethod  reset-shaft  ((s  shaft)) 

(set-shaft  s  U  0) 

(setf  (time-stamp  s)  (system-time))) 

(defmethod  connect  ((source  shaft)  (load  shaft)) 

(setf  (time-stamp  load)  (time-stamp  source)) 

(set-shaft  load  (theta  source)  (omega  source))) 
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;  *'motor.class.cl" 


(defclass  motor  (shaft) 

((torque-constant  ;  Newton-meters/ampere 
:initarg  ;Kt 
;accessor  Kt 
;type  float) 

(back-emf-constant  ;  Volts/(rad/sec) 

:initarg  ;Kb 
;  accessor  Kb 
;t>pe  float) 

(armature-resistance  ;  ohms  (must  be  >  0) 

:initarg  ;R 
:  accessor  R 
:inilform  1 
:type  float) 

(max-brush-drop  ;  volts 
;initarg  :  max-brush-drop 
:accessor  max-brush-drop 
:initform  2.0 
:type  float) 

(half-brush-drop-source-value ;  volts 
linitarg  ;half-BD-value 
laccessor  half-BD-value 
;initform  3.0 
;type  float) 

(armature-current  ;  amperes  (saved  for  feedback  purposes) 

;accessor  armature-current 
:initform  0 
;type  float))) 

(deftnethod  applied-voltage  ((m  motor)  source-voltage) 

(if  (zerop  source-voltage)  0 
(•  source-voltage 
(-1 

(*  (/  (max-brush-drop  m)  (abs  source-voltage)) 

(-1 

(exp  (•  (log  0.5) 

(/  (abs  source-voltage) 

(half-BD-value  m)))))))))) 

(deftnethod  developed-torque  ((m  motor)  source-voltage) 

(setf  (armature-cunent  m) 

(/  (-  (^lied-voltage  m  source-voltage)  (*  (Kb  m)  (omega  m)))  (R  m))) 
(*  (Kt  m)  (armature-current  m))) 
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(definethod  omega-dot  ((m  motor)  source-voltage  load-inertia  load-torque 
load-coulomb-fhction-constant 
load-viscous-friction-constant) 

(let*  ((torque  (-)■  (developed-torque  m  source-voltage)  load-torque)) 
(Fc-total  {+  (Fc  m)  load-coulomb-fhction-constant)) 

(friction-loss 
(if  (zerop  (omega  m)) 

(if  (zerop  torque) 

0 

(if  (>  Fc-total  (abs  torque)) 
torque 

(•  Fc-total  (sgn  torque)))) 

(+  (•  (+  (Fv  m)  load-viscous-friction-constant)  (omega  m)) 

(*  Fc-total  (sgn  (omega  m))))))) 

(/  (-  torque  friction-loss)  (+  (I  m)  load-inertia)))) 

(defmethod  run-motor  ((m  motor)  source-voltage  load-inertia  load-torque 
load-coulomb-friction-constant 
load-viscous-friction-constant) 

(let  ((dt  (delta-time  (time-stamp  m))) 

(omega-dot  (omega-dot  m  source-voltage  load-inertia  load-torque 
load-coulomb-friction-constant 
load-viscous-friction-constant))) 

(self  (theta  m)  (+  (theta  m)  (•  (omega  m)  dt))) 

(setf  (omega  m)  (+  (omega  m)  (*  omega-dot  dt))) 

(setf  (time-stamp  m)  (+  (time-stamp  m)  dt)))) 
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;  ''rcduction>gear.class.cr' 


(defclass  reduction-gear  () 

((gear-ratio 
linitarg  ;gear-ratio 
:  accessor  gear-ratio 
:initfonn  1 
:t\pe  float) 

(in-shaft 
;initarg  :  in-shaft 
:acce$sor  in-shaft 
:initform  (make-instance ’shaft)) 

(out-shaft 
:initarg  :out-shaft 
:accessor  out-shaft 
iinitform  (make-instance  ’shaft)))) 

(deftnethod  rg-reduce  ((rg  reduction-gear)  value) 

(/  value  (gear-ratio  rg))) 

(defmethod  rg-reflect  ((rg  reduction-gear)  value) 

(•  value  (gear-ratio  rg))) 

(defmethod  rg-inertia-forward  ((rg  reduction-gear)  inertia-value) 
(•  inertia-value  (sqr  (gear-ratio  rg)))) 

(defmethod  rg-inertia-backward  ((rg  reduction-gear)  inertia-value) 
(/  inertia-value  (sqr  (gear-ratio  rg)))) 

(defmethod  rg<oiuiect  ((rg  reduction-gear)) 

(set-shaft  (out-shaft  rg) 

(rg-reduce  rg  (theta  (in-shaft  rg))) 

(rg-reduce  rg  (omega  (in-shaft  rg))))) 
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;  "joint.class.cr' 


(defclass  joint  () 

((pulse^ounter 
linitarg  :pc 
laccessor  pc 

initform  (make-instance  'di£f<ounter)) 

(driver 

:initarg  :driver 
;  accessor  driver 

:initform  (make-instance  'motor-driver)) 

(prime-mover 
:initarg  :  prime-mover 
accessor  prime-mover 
:initform  (make-instance  ’motor)) 

(red-gear 
;initarg  :  red-gear 
;  accessor  red-gear 

:  initform  (make-instance  ’reduction-gear)) 

(load-shaft 
:initarg  :  load-shaft 
:accessor  load-shaft 
:initform  (make-instance ’shaft)))) 

(deftnethod  motor-load-inertia  (0  Joint)) 

(+  (I  (in-shaft  (red-gear  j))) 

(rg-inerlia-backward  (red-gear  j)  (+  (I  (out-shaft  (red-gear  J))) 
(I  (load-shaft  j)))))) 

(deftnethod  motor-load-coulomb-friction-constant  ((j  joint)) 

(+  (Fc  (in-shaft  (red-gear  j))) 

(rg-reduce  (red-gear  j)  (+  (Fc  (out-shaft  (red-gear  j))) 

(Fc  (load-shaft  j)))))) 

(deftnethod  motor-load-viscous-ftriction<onstant  ((j  joint)) 

(+  (Fv  (in-shaft  (red-gear  j))) 

(rg-reduce  (red-gear  j)  (+  (Fv  (out-shaft  (red-gear  j))) 

(Fv  (load-shaft  j)))))) 

(deftnethod  feedback  ((j  joint)) 

(-  (theta  (out-shaft  (red-gear  j)))  (theta  (load-shaft  j)))) 
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(definethod  ijicreiiiem-j<^t  ((j  joint)  older) 

(nin-mottv  (prime-niover  j) 

(drive  (driver  j)  (diff-signal  (pc  j)  order  (feet&ack  j)) 

(*  .003  (RAD/SECtoRPM  (omega  (prime>mover  j)))) 
(aniiature<unent  (prime-mover  j))) 
(motor-load-ineitia  j) 

0  ;  load  not  producing  any  torque 
(motor-load-coulomb-friction-constant  j) 

( motor-load-viscous-friction-constant  j)) 

(connect  (out-shaft  (red-gear  j))  (load-shaft  j)) 

(connect  (prime-mover  j)  (in-shaft  (red-gear  j))) 

(rg-connect  (red-gear  j))) 

(defmethod  step-input-to-joint  ((j  joint)  step) 

(diff-signal  (pc  j)  (REVtoRAD  step)  0)) 

(defrnethod  reset-joint  ((j  joint)) 

(setf  (cunent-count  (pc  j))  0) 

(reset-shaft  (load-shaft  j)) 

(reset-shaft  (out-shaft  (red-gear  j))) 

(reset-shaft  (in-shaft  (red-gear  j))) 

(reset-shaft  (prime-mover  j))) 
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;  "jointinstance.cl" 


(self  joiml  (make-instance  'joint 

driver  (make-instance  'motor-driver 
:amplification-&ctor  1 
;  max-value  75  ;  volts 
min-value  -75  ;  volts 
:  displacement-gain  150 

:velocity-fb-gain  5 
:current-fb-gain  .5 ) 
prime-mover  (make-instaitce  'motor 
:I  .0005  ;  Kg-m*m 

:Fc  .0075  ; N-m 

:Fv  .00004  ;  N-in/( rad/sec) 

:Kt  .005  ;  N-m/ampere 

:Kb  .255  ;  VoIts/(rad/sec) 

:R  1  ) ;  ohms 

: red-gear  (make-instance  'reduction-gear 
:  gear-ratio  200) 

:  load-shaft  (make-instance 'shaft 
;I  5  ;  Kg-m*m 

;Fc  .1  ;N-m 
:Fv  .02  ))) ;  N-m/(rad/sec) 

(defim  move-joint  (delta-theta) 

(setf  (time-stamp  (prime-mover  joint  1))  (system-time)) 

;  input  delta-theta  a  little  at  a  time 
(if  (<  delta-theta  0) 

negative  delta-theta  (use  -0.015  steps) 

(do*  ((input-index  0  (+  input-index  0.015))) 

((<  (-*-  delta-theta  input-index)  0.015) 

(step-input-to-joint  joint  1  (-•-  delta-theu  input-index))) 
(step-input-to-joint  joint  1  -0.015) 

(increment-joint  jointl  0) 

(display-state  *display*  jointl  (time-stamp  (prime-mover  jointl)))) 
;positive  delta-theta  (use  0.015  steps) 

(do*  ((input-index  0  (+  input-index  0.015))) 

((<  (-  delta-theta  input-index)  0.015) 

(step-input-to-joint  jointl  (-  delta-theta  input-index))) 
(step-input-to-joint  jointl  0.015) 

(increment-joint  jointl  0) 

(displ^-state  *display*  jointl  (time-stamp  (prime-mover  jointl))))) 
;delta-theta  entry  into  PC  is  complete 
;cycle  until  ordered  position  is  reached 
(do*  ((index  1)) 

((and  (<  (abs  (current-count  (pc  jointl)))  0.05) 

(<  (abs  (omega  (prime-mover  jointl)))  10))  (pprint  'stop)) 
(increment-joint  jointl  0) 

(displa> -state  *display*  jointl  (time-stamp  (prime-mover  jointl))))) 
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(defun  move-joint-mult  (delta-theta  mult) 

(clear-and-reset) 

(dotimes  (i  mult)  (move-joint  delu-thett))) 

(defim  run^int  (speed) 

(clear-and-reset) 

(do*  ((dtime  (delta-time  (time-stamp  (prime-mover  Jointl))) 
(delta-time  (time-stamp  (prime-mover  jointl))))) 

((<  (abs  (-  (RPMtoRAD/SEC  speed)  (omega  (load-shaft  jointl)))) 

0. 1)  (pprint  'stop)) 

(increment-joint  jointl  (*  dtime  (RPMtoRAO/SEC  speed))) 
(display-state  ^display*  jointl  (time-stamp  (prime-mover  jointl))))) 

(defun  move-joint-list  (delta-theta-list) 

(clear-and-reset) 

(if  (equalp  nil  delta-theta-list)  nil 
(move-joint-list-2  delta-theta-list))) 

(defun  move-joint-list-2  (delta-theta-list) 

(move-joint  (car  delta-theta-list)) 

(if  (equalp  nil  (cdr  delta-theu-list))  nil 
(move-joint-list-2  (cdr  delta-theta-list)))) 

(defiui  clear-and-reset  () 

(update-minutes  *display*  0) 

(reset-system-time) 

(reset-joint  jointl)) 


;  "window.instaiice.cr' 


.  dimensions  for  x-y  coord  system  (window  size  auto  adjusts) 
(setf  *x-origin*  50) 

(setf ‘x-length*  500) 

(sctf  *x-tics*  6) 

(setf  ‘y-origin*  50) 

(setf  *y-length*  340) 

(setf ‘y-tics*  8) 

(setf  *max-q)eed*  4000) ;  (max  rpm's  of  motor  scale) 

(setf  *max-revs*  1000) ;  (max  rev's  of  motor  scale) 

(requite  ;xcw) 

(use-package  ;cw) 

(cw;  initialize-common-windows) 
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(deftMthod  draw-grid  ((window  window-stream)) 

(draw-line-xy  window  ‘x-origin*  ;top  border 

(+  *y-origin*  •y-length*) 

(+  •x-origin*  •x-Iengih*) 

(+  •y-origin*  •y-length*)) 

(draw-line-xy  window  •x-origin*  ;nuddle  x  axis 

(+  ‘y-origin*  (/  •y-Iength*  2)) 

(■*■  •x-origin*  *x-length*) 

(f  •y-origin*  (/  *y-Icngth*  2))) 

(draw-line-x>’  window  *x-origin*  ibottom  border 

•y-origin* 

(+  •x-origin*  •x-length*) 

•y-origin*) 

(draw-line-xy  window  •x-origin*  ;left  border 

•y-origin* 

•x-origin* 

(-*-  •y-origin*  •y-length*)) 

(draw-line-xy  window  (+  •x-origin*  *x-length*)  ;right  border 
•y-origin* 

(+  •x-origin*  •x-length*) 

(+  *y-origin*  •y-length*)) 

(dotimes  (i  *x-tics*)  ;  mark  x  axis 

(draw-line-xy  window  (+  •x-origin* 

(/  (•  i  *x-length*)  *x-tics*)) 

•y-origin* 

(+  *x-origin* 

(/  (*  i  *x-length*)  *x-tics*)) 

(+  •y-origin*  *y-length*) 

:dashing  '(1  3))) 

(dotimes  (i  •y-tics*)  ;  mark  y  axis 

(draw-line-xy  window  *x-origin* 

(+  •y-origin* 

(/  (*  i  *y-length*)  *y-tics*)) 

(+  *x-origin*  *x-length*) 

(+  •y^rigin* 

(/  (*  i  *y-length*)  *y-tics*)) 

:d^hing  '(1  3))) 

(label-graph  window)) 
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(definethod  label*graph  ((w  window-stream)) 

;  theta  labels 

(setf  (window-strcam-y-position  w)  (+  •y-origin*  *>'-length*  -4)) 

(setf  (window-stream-x^wsition  w)  (+  ‘x-origin*  -35)) 

(setf  (window-stream-foreground-color  w)  red) 

(format  w  "-s"  *inax-revs*) 

(setf  (window-stream-x-position  w)  (+  •x-origin*  ‘x-length*  5)) 

(setf  (window-stream-foreground-color  w)  blue) 

(format  w  "+1") 

(setf  (window-stream-y-position  w)  (+  ‘y-origin*  (*  *y-length*  0.75)  -3)) 
(setf  (window-stream-x-position  w)  (+  ‘x-origin*  -40)) 

(setf  (window-stream-foreground-color  w)  black) 

(format  w  "Revs") 

(setf  (window-stream-x-position  w)  (+  ‘.x-origin*  ‘x-length*  5)) 

(format  w  "Revs") 

(setf  (window-stream-y-position  w)  (+  ‘y-origin*  (*  *y-length‘  0.5)  3)) 
(setf  (window-stieam-x-position  w)  (+  ‘x-origin*  -43)) 

(setf  (window-stream-foreground-color  w)  red) 

(format  w  "-s"  (-  ‘max-revs*)) 

(setf  (window-stream-.x-position  w)  (+  ‘x-origin*  ‘x-length*  5)) 

(setf  (window-stream-foreground-color  w)  blue) 

(format  w  "-1") 

;  omega  labels 

(setf  (window-stream-y-position  w)  (+  ‘y-origin*  (*  ‘y-length*  0.5)  -10)) 
(setf  (window-stream-x-position  w)  (+  ‘x-origin*  -35)) 

(setf  (window-stream-foreground-color  w)  red) 

(format  w  "~s"  ‘max-speed*) 

(setf  (window-stream-x-position  w)  (+  ‘x-origin*  *x-length‘  10)) 

(setf  (windo'v-stream-foreground-color  w)  blue) 

(format  w  "~s"  (/  ‘max-speed*  100)) 

(setf  (window-stream-y-position  w)  (+  ‘y-origin*  (*  ‘y-length*  0.25)  -3)) 
(setf  (window-stream-x-position  w)  (+  ‘x-origin*  -36)) 

(setf  (window-stream-foreground-color  w)  black) 

(format  w  "RPM") 

(setf  (window-stieam-x-position  w)  (+  ‘x-origin*  *x-length*  5)) 

(format  w  "RPM") 

(setf  (window-stream-y-position  w)  (+  ‘y-origin*  3)) 

(setf  (window-stream-x-position  w)  (+  ‘x-origin*  -43)) 

(setf  (window-stream-foreground-color  w)  red) 

(format  w  "-s"  (-  ‘max-speed*)) 

(setf  (window-stream-x-position  w)  (+  ‘x-origin*  ‘x-iength*  5)) 

(setf  (window-stream-foreground-color  w)  blue) 

(format  w  "~s"  (-  (/  ‘max-speed*  100))) 

;  time  labels 

(setf  (window-stream-foreground-color  w)  black) 

(setf  (window-stream-y-position  w)  (+  ‘y-origin*  -15)) 

(setf  (window-stream-x-position  w)  (+  ‘x-origin*  -3)) 

(format  w  "0") 

(setf  (window-stream-x-position  w)  (+  ‘x-origin*  (/  ‘x-length*  6)  -7)) 
(format  w  "10") 

(setf  (window-stream-x-position  w)  (+  ‘x-origin*  (/  ‘x-length*  3)  -7)) 
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(format  w  "20") 

(self  (window-stream-x-position  w)  (+  •x-origin*  (/  *x-length*  2)  -7)) 
(format  w  "30") 

(setf  (window'Stream-X'position  w)  (+  •x-origin*  (*  *x-length*  (/  2  3))  -7)) 
(format  w  "40") 

(setf  (window-stream-X'position  w)  (+  •x-origin*  (♦  •x-length*  (/  5  6))  -7)) 
(format  w  "50") 

(setf  (window-stream-x-position  w)  (+  •x-origin*  •x-length*  -7)) 

(format  w  "60") 

(setf  (window -strcam-y-position  w)  (+  •y-origin*  -30)) 

(setf  (window-stream-x-position  w)  (+  •x-origin*  -35)) 

(setf  (window -stream-foreground-color  w)  red) 

(format  w  "motor") 

(setf  (window -stream-x-position  w)  (+  •x-origin*  •x-length*)) 

(setf  (window -stream-foreground-color  w)  blue) 

(format  w  "joint") 

(setf  (window-stream-foreground-color  w)  black) 

(setf  (window-stream-x-position  w)  (+  •x-origin*  (•  •x-length*  .5)  -13)) 
(format  w  "time")) 

(defmethod  draw-motor-position  ((window  window-stream)  revolutions) 
(draw-point-xy  window 
•x-time-value* 

(+  ‘y-origin*  (*  0.75  ’y-length*) 

(*  0.25  *y-length* 

(cond  ((zerop  revolutions)  0) 

((<  revolutions  0) 

(/  (-  (mod  revolutions  *max-revs*) 

•max-revs*) 

•max-revs*)) 

(t  (/  (mod  revolutions  *max-revs*) 

•max-revs*))))) 

;color  red)) 

(defmethod  draw-load-position  ((window  window-stream)  revolutions) 
(draw-point-.xy  window 
•x-time-value* 

(+  *y-origin*  (*  0.75  *y-length*) 

(*  0.25  *y-lcngth* 

(cond  ((zerop  revolutions)  0) 

((<  revolutions  0) 

(-  (mod  revolutions  1.0)  1.0)) 

(t  (mod  revolutions  1.0))))) 
icolor  blue)) 
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(definethod  draw-motor-speed  ((window  window-stream)  speed) 
(draw-point-xy  window 
•x-timc-value* 

(+  •y-origin*  (*  0.25  •y-length*) 

(*  0  25  •y-length*  (/  speed  •max-speed*))) 

;color  red)) 

(definethod  draw-load-speed  ((window  window-stream)  speed) 
(draw-point-.\\'  window 
•x-time-value* 

(+  •y-origin*  (•  0.25  •y-length*) 

(•  0.25  •y-length*  100  (/  speed  •max-speed*))) 
xolor  blue)) 

(defmethod  update -minutes  ((window  window-stream)  minutes) 

(clear  window) 

(draw-grid  window) 

(self  *minutes*  minutes)) 

(defmethod  set-x-coord  ((window  window-stream)  seconds) 

(if  (>  (truncate  (/  seconds  60))  *minutes*) 

(update-minutes  window  (truncate  (/  seconds  60)))) 

(setf  •x-time-value*  (round  (+  •x-origin* 

(*  (/  (mod  seconds  60)  60) 

•x-length*))))) 

(defmethod  display-state  ((window  window-stream)  (j  joint)  current-time) 
(set-x-coord  window  current-time) 

(draw-motor-speed  window  (RAD/SECtoRPM  (omega  (prime-mover  j)))) 
(draw-load-sp^  window  (RAD/SECtoRPM  (omega  (load-shaft  j)))) 
(draw-motor-position  window  (RADtoREV  (theta  (prime-mover  j)))) 
(draw-load'position  window  (RADtoREV  (theta  (load-shaft  j))))) 

(setf  •display* 

(make-window-stream 
left  1 
ibottom  1 

:width  (+  •x-length*  (•  2  •x-origin*)) 

:  height  (+  •y-length*  (•  2  •y-origin*)) 

;background-coIor  white 

:foreground-color  black 

;inner-region-left 

:  inner-region-bottom 

:inner-region-width 

:  inner-region-height 

;title  "joint  actuator  simulation" 

;activate-p  t)) 

(setf  •minutes*  0) 

(draw-grid  •display*) 
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;  ''tiinc.routines.cr' 


(defiin  reset-system^time  () 

(self  •RefTime*  (get*intenial'real*time))) 

(defiut  system-time  () 

(/  {-  (get-internal-real-time)  •RefTime*)  1000.0)) 

(defim  delta-time  (time) 

(-  (system-time)  time)) 

( reset-system-time) 


;  ''math.routines.cr' 


(defim  sqr  (x)  (•  x  x)) 


(defim  sgn  (x) 

(if(<x0)-l  D) 

(defim  RPMtoRAD/SEC  (rpm) 

(•  rpm  (/  pi  30))) ;  •  2pi/60 

(defim  RAD/SECtoRPM  (rad/sec) 
(•  rad/sec  (/  30  pi))) 

(defim  REVtoRAD  (rev) 

(•  2  pi  rev)) 

(defim  RADtoREV  (rad) 
(/rad(*2pi))) 
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APPENDIX  B 


SOURCE  CODE  FILES  (untested) 


//  file  "motor.h" 


#ifndef_MOTOR_H 
#define _ MOTOR_H 

#include  <math.h> 

^include  <stdio.h> 

class  motor  {  //  inertia-less  motor  class 
private; 

float  Fc;  //  Coulomb  friction  constant 
float  Fv;  //  Viscous  friction  constant 
float  Kt;  //  Torque  constant 
float  Kb;  //  Back  EMF  constant 
float  Ra;  //  Armature  resistance 
float  BDm;  //  Rated  brush  brop  value 
float  BDc;  //Ln(l/2)/BDh 

//  BDh  =  Voltage  applied  such  that  brush  drop  =  1/2  BDm 
//  subtracts  brush  drop  from  source  voltage 
float  AppliedVoltage(float); 

public; 

float  la;  //  Armature  cunent  (available  for  current  feedback) 
motorO  { } 

//  called  after  constructor  for  initialization 
void  init_motor(float  TorqueConstant,  //  N*m/ Ampere 

float  BackEMFConstant,  //  Volts/RPM 
float  NoLoadCurrent,  //  Amperes 

float  NoLoadSpecd  =  1000.0,  //  RPM 
float  StartingCurrent  =  0.0,  //  Amperes 
float  ArmatureResistance  -  1 .0,  //  Ohms 

float  RatedBrushDrop  =  2.0,  //  Volts 
float  HalfBrushDrop  =  3.0)y/Volts 
float  DevelopedTorque(float.  float); 

}; 
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//  provide  initialization  for  specific  motor  types  in  Aquarobot 
void  makeRA20(niotor&  m); 
void  makeRH2S(motor&  m); 

#endif 
/•  EOF  •/ 


//  file  "motor.c" 


#include "  motor,  h" 

void  motor:  :init_motor( float  TorqueConstant. 
float  BackEMFConstant. 
float  NoLoadCurrent, 
float  NoLoadSpeed, 
float  StartingCurrent 
float  ArmatureResistance, 
float  RatedBrushDrop, 
float  HalfBrushDrop) 

{ 

la  =  0.0; 

Fc  =  TorqueConstant  •  StartingCurrent; 

Fv  =  (TorqueConstant  •  NoLoadCurrent  -  Fc)  /  NoLoadSpeed; 

Kt  =  TorqueConstant; 

Kb  =  BackEMFConstant; 

Ra  =  ArmatureResistance; 

BDm  =  RatedBrushDrop; 
if(BDm<0.0){ 

printf("error:  rated  brush  drop  must  be  >=  0  Volts... \n"); 
printffdefault  brush  drop  value  (0  Volts)  used...\n"); 

BDm  =  0.0; 

BDc=  1.0; 

} 

else  if  (BDm  =  0.0)  { 

BDc=  1.0; 

} 

else  if  (HalfBrushDrop  <  BDm/2)  { 
printf("error:  half  brush  drop  must  be  >=  1/2  rated  brush  drop...\n"); 
printfCdefaulted  to  1/2  RatedBrushDrop. .\n"); 

BDc  =  log(0.5)  •  2.0  /  BDm; 

} 

else  { 

BDc  =  log(0.3)  /  HalfBrushDrop; 

}’ 


77 


float  motor:  ;DevelopedTorque(float  Source  Voltage,  float  omega) 

{ 

float  Torque; 
float  FrictionLoss; 

//  determine  armature  current  and  save 
la  (AppliedVoltage(SourceVoltage)  •  (Kb  *  omega))  /  Ra; 

//  determine  motor  torque 
Torque  =  Kt  •  la; 

//  calculate  loss 

//  loss  opposes  omega  (viscous  and  coulomb  components) 
if  (omega  >  0.0)  FrictionLoss  =  omega  •  Fv  +  Fc; 
else  if  (omega  <  0.0)  FrictionLoss  =  omega  •  Fv  -  Fc; 

//  if  (omega  =  0) :  loss  opposes  Torque  (no  viscous  component) 
else  if  (Torque  >  Fc)  FrictionLoss  =  Fc; 
else  if  (Torque  <  -Fc)  FrictionLoss  =  -  Fc; 

//  if  ((omega  ==  0)  &&  (|Torque|  <  Fc)) :  Torque  insufiicient  to  overcome  Fc 
else  FrictionLoss  =  MotorTorque; 

return  (Torque  -  FrictionLoss); 

} 


78 


/*  Private  Function  */ 


float  motor:; Applied Voitage(float  Vs) 

{ 

//  return  Vs(l  -  (BDm/|Vs|)*(l  -  exp(ln(l/2)*|Vs|/BDh))) 

if(Vs==0.0){ 
return  0,0; 

} 

else  if  (Vs  <  0.0)  { //  negative  Vs 
return  (Vs  +  (BDm  *  ( I  -  exp(-BDc* Vs)))); 

} 

else  {  //  positive  Vs 

return  (Vs  -  (BDm  *  ( 1  -  exp(  BDc* Vs)))); 

} 

} 


/*  specific  Aquarobot  motor  tvpe  initializations  */ 

//  Aquarobot  motor  parameters  for  init_motor 

//  read  from  spec  sheet  provided;  includes  harmonic  gear 

#define  RA20_PARAMETERS  32.0,  3.4.  0.78.  25.0, 0.32.  3  2.  2.0,  3.0 

#define  RH25_PARAMETERS  33.0,  3.5,  0.89,  25.0. 0.48,  l.l,  2.0,  3.0 

void  makeRA,'0(motor&  m) 

{ 

m.init  motor(RA20  PARAMETERS); 

} 

void  makeRH25(motor&  m) 

{ 

m.  init_motor(RH25_PARAMETERS); 

} 

/•  EOF  •/ 


79 


//  file  ''amplifier_clipper.h'' 


#ifndef  _AMP_CLIP_H_ 

^define  _AMP_CLIP_H_ 

class  ainplifier_clipper  { 

private: 
float  gain; 
float  max_value; 
float  min_value; 

public: 

amplifler_clipper()  { } 

//  call  after  constructor  for  initialization 

void  init_ainplifier_clipper(float  g.  float  max_v,  float  inin_v); 

float  ampli^’ffloat  input_value); 


class  aqua_driver  :  private  ainplifier_clipper  { 
private: 

float  theta_gain; 
float  omega_gain; 
float  current_gain; 

public: 

aqua_driver()  { } 

//  call  after  constructor  for  initialization 
init_aqua_driver(float  displacement _gain, 
float  velocity_gain, 
float  current.feedback .^n); 
float  drive(float  theta_eiTor,  float  omega,  float  current); 
}; 

//  provide  initialization  for  specific  joint  on  aqualeg 
void  makeJldriver(aqua_dtiver&  d); 
void  makeJ2driver(aqua_dtiver&  d); 
void  makeJ3driver(aqua_dtiver&  d); 

#endif 
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//  fUe  "amplifier^clippcr.c" 


iinclude  "amplifier^cUpper.h” 
void 

init_ainplifier_clipper(float  g.  float  max_v.  float  inin_v) 

{ 

gain  =g; 
max_value  =  ma\_v; 
minvalue  =  min_v; 

} 

float 

ainplifier_clipper:  iampIifyCfloat  inputvalue) 

{ 

float  output_value  =  gain  •  input_value; 

if  (niin_value  >  output_value) 
return  nun_value; 
elsif  (max_value  <  output_value) 
return  inax_value; 
else 

retiun  output  value; 

} 

aqua_driver;;init_aqua_driver(float  displacement .gain, 
float  velocity .gain, 
float  current  feedback .gain) 

{ 

//  set  flnal  gain  to  1  and  clip  at  +/•  73  VDC 
init.amplifler.clipper(1.0,  75.0.  -75.0); 

theta_gain  =  displacement .gain; 
omega.gain  =  velocity.gain; 
current .gain  =  current.feedback .gain; 

} 

float 

aqua.driver;:drive(float  theta.error,  float  omega,  float  current) 

{ 

return  (amplify(  theta .gain  *  theta.error 
•omega .gain  *  omega 
+  current .gain  *  current)); 

} 
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//  driver  specs  (theta,  omega,  current) 

#deane  SHOULDER  DRIVER_GAINS  ISO  0.  5  0. 0  5 
#define  KNEEl  DRIVER  GAWS  150  0.  5  0. 0.5 
^define  KNEE2_DRIVER_GAINS  150.0.  5  0. 0.5 

void  inakeJldhver(aqua_driver&  d) 

{ 

d.  init_aqua_drivcr(  SHOULDER_DRI  VER_GAINS); 

} 

void  n)akeJ2driver(aqua  driver&  d); 

{ 

d.init_aqua_driver(KNEEl_DRIVER_GAINS). 

} 

void  malceJ3driver(aqua_driver&  d); 

{ 

d.init_aqua_driver(KNEE2_DRIVER_GAINS); 

} 


//  file  ''joint_actuator.h'' 


iiifndef _ JA_H _ 

Wefine  _Ja1h_ 

#include  o 
#include "" 

class  aquaJoint_actuator  { 
protected: 

float  ordeied_theta; 
ampltflerclipper  da_coaverter. 
amplifler_clipper  fv_converter; 
aqua_driver  d; 
motor  m; 

public: 

aquajoint_actuator0  (} 

void  reset(float  theta); 

void  input_order(float  delta.theta); 

float  torque(float  current  th^  float  current  omega); 

}; 


class  shoulder_actuator :  public  aquaJoint_actuator  ( 
public: 

shoulder_actuator(float  them); 

}; 
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class  kneel.actuator ;  public  aquaJoint_actuator  { 
public; 

kneel_actuator(float  theta); 

float  torque(float  cunent_theta,  float  cunent_oinega); 

>; 

class  knee2_actuator ;  public  aquaJoint_actuator  { 
public: 

knee2_actuator(float  theta); 

float  torque(float  cunent_theta.  float  cuiTent_omega); 

K 

#endif 


//  file  "joint_actuator.c" 


#include  ''joint_actuator.h'’ 

//  aquaJoint_actuator  (parent  class)  functions 
void 

aqua  Joint  actuator;  :reset(float  theta); 

{ 

ordered_theta  =  theta; 
m.la  =  0.0; 

> 

void 

aqua  Joint  actuator:;input_order(float  delta  theta); 

{ 

orderedtheta  +=  delta_theta; 

}  V 

float 

aqua  Joint  actuator;  ;torque(float  current  theta,  float  current.omega); 

{ 

float  source_voitage; 
source^voltage  =  d->drive 

(da_converter->ainplify(current_theta  •  ordered_theta), 

fv_converter->amplify(current_oniega), 

m-Ia); 

return  (m->developed  torque(source_voltage)); 

} 
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//  (kriived  class  specs 


//  output  10  volts  for  6 144  pulse  count 
#define  DA_CONVERTER_RATIO  10.0/6144.0 

// 100  pulses  drive  the  motor  1  revolution 
#define  PULSES_PER_REV  100.0 

//  output  3  volts  per  1000  RPMs 

#<lefme  FV_CONVERTER_RATIO  3.0/1000.0 

//  harmonic  gear  otdy  for  shoulder 
#define  SHOULDER_REDUCTION  161.0 

//  harmonic  and  bevel  gears  for  knees 
#define  KNEE1_REDUCTI0N  160.0*3. 0 
#define  KNEE2_REDUCT10N  160.0*2.0 

//  derrived  class  functions 

shoulder_actuator;  ;shoulder_actuator(float  theta) 

{ 

ordered^theta  =  theta: 
da  converter*>init  ampiifier_clipper 

(SHOULDER_REDUCTION  *  PULSES_PER_REV  *  DA_CONVERTER_RATIO.  10.0,  -10.0); 
fv  converter->init  amplifier  clipper 

(SHOULDER_REDUCnON  *  FV_CONVERTER_RATIO,  10.0,  -10.0); 
makeJldriver(&d); 
makeRA20(&m); 

} 

kneel_actuator;;kneel_actuator(float  theta) 

{ 

ordered_theta  =  theta; 
da  converter'>init_amplifier_clim)er 

(ibJEEl_REDUCnON  *  PULSES_PER_REV  *  DA_CONVERTER_RATIO,  10.0,  -10.0); 
fv  convener->init_amplifier_cUpper 
{I64EEl_REDUCnON  *  ^_CONVERTER_RATIO,  lO.O,  -lO.O); 
makeJ2driver(&d); 
makeRH23(&m); 

} 

float 

kneel_actuator::torque(float  current_theta,  float  current.omega) 

{ 

return  (3.0  *  aquaJoint_actuator;;torque(current_theta,  current_omega/3.0)); 

} 
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kneel  actuator;  ;knee2  actuator(float  theta) 

{ 

ordered_theta  *  theta; 
da_conveiter->iiut  amplifier  clipper 

(KNEE2_REDUCTION  •  PULSES_PER_REV  •  DA_CONVERTER_RATIO.  10.0.  -10.0); 
fv  converter->iiiit  amplifier  clipper 
(KNEE2_REDUCnON  •  ^_CONVERTER_RATIO.  10.0.  -10.0); 
makeJ3dhver(&d); 
ni!dceRH23(&m); 


float 

knee2_actuator;;torque(float  currentjheta.  float  current^omega) 

{ 

return  (2.0  •  aquaJoint_acniator::torque(cuiTenC_theta.  current_omega/2.0)); 

} 
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APPENDIX  C 


LOADING  AND  OPERATING  INSTRUCTIONS 


To  run  demo,  start  LISP  Interpreter  and  call: 

(load  "aqua-loader") 

This  file  loads  the  source  code  in  the  correct  sequence  and  makes  calls  to  run  the  demo. 
Additional  tuns  may  be  observed  by  calling; 

(drop-aqua) 


SOURCE  CODE  FILES 


;  "aqua-ioader'* 

;  LOADER  FOR  AQUA-ROBOT 

;  define  loader/compiler  functions 

(load  "load-files.d") 

;  aqua-robot  loader/compiler  fimctions 

;  (load-aqua) 

(load-compiled-aqua) 

;  (compile-and-load-aqua) 

(defun  drop-aqua  () 

(restart-aqua) 

(do  0  (nil) 

(dotimes  (i  *loops*)  (update-aquarobot  aqua-1)) 
(new-picture))) 

(aqua-picture) 

(drop-aqua) 
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;  "Io«d-(iles.ci" 


(defiin  load-aqua  () 

;  general  purpose  files 
(load  "misc.cr) 

(load  "vector  cl") 

(load  "matrix.cl") 

(load  "kinematics.cr') 

(load  "rigid-body.cl") 

(load  "strobe-camera-cr) 

(load  "link-cl") 

;  aqua-robot  specific  files 
(load  "aqua-data-d") 

(load  "aqua-link.cl") 

(load  "aquaxl") 

(load  "aqua-leg.cl'') 

(load  "aqua-inv-kinematics.cl") 

(load  "aqua-jacobian.cr) 

(load  "aqua-update-forces-and-torques.d")) 

(defiin  load-compiled-aqua  () 

(load  "misc.fasl") 

(load  "vector.fasl") 

(load  "matrix.fasr) 

(load  "kinematics-fasl") 

(load  "rigid-body-fasl") 

(load  "strobe-camerafasl") 

(load  "linkfasD 
(load  "aqua-dataTasr) 

(load  "aqua-link.fasl") 

(load  "aqua-fasl") 

(load  "aqua-leg.fasl") 

(load  "aqua-inv-kinematics-fasl”) 

(load  "aqua-jacobian.fasl") 

(load  ”aqua-update-forces-and-torques.&sr)) 
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(<teftin  compile>and>load*aqua  () 

;  (compilC'file ‘'misc.cD 
(load  "misc.fiur) 

;  (compile-file  "vector.cl") 

(load  "vector,  fasl") 

;  (compile-file  "matrix  cl") 

(load  "matrix.fasl") 

(compile-file  "kinematics  cl") 

(load  "kinematics,  fasl") 

(compile-file  "rigid-body  cl") 

(load  "rigid-bodv’.fasr) 

(compile-file  "strobe-camera.d") 

(load  "strobe-camera. fasl") 

(compile-file  "link.cl") 

(load  "link.fasl") 

(compile-file  "aqua-data.cr) 

(load  "aqua-data-fasl") 

(compile-file  "aqua-link.cl") 

(load  "aqua-link.fasl") 

(compile-file  "aqua.cr) 

(load  "aqua.fasr) 

(compile-file  "aqua-leg.cl") 

(load  "aqua-leg.fasl") 

(compile-file  "aqua-inv-kinematics.cl") 

(load  "aqua-inv-kinematics.fasl") 

(compile-file  "aqua-jacobian.d") 

(load  "aqua-jacobian.fasl") 

(compile-file  "aqua-update-forces-and-torquescl") 
(load  "aqua-update-forces-and-torques.£asr)) 
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;  "misc.cl” 


(defun  auin2  (dx  dy) 

(cond  ((zerop  dx)  (cond  ((zerop  dy)  0.0) 

((<dyO)  (-(*0.5  pi))) 

0  (*  0.5  pi)))) 

((<dxO)  (cond{(<dyO)  (- (atan  (/ dy  dx))  pi)) 
(t  (+  (atan  (/  dy  dx))  pi)))) 

(t  (atan  (/  dy  dx))))) 

;  returns  angle  in  degrees. 

(defun  atan2d  (dx  dy)  (rad-to-deg  (atan2  dx  dy))) 

(defun  sqr  (x)  {*  x  x)) 

(defconstant  rad-to-deg-multiplier  (/  180  pi)) 

(defun  rad-to-deg  (rad)  (•  rad  rad-to-deg-multiplier)) 

(defconstant  deg-to-rad-multiplier  (/  pi  180)) 

(defiiii  deg-to-rad  (deg)  (*  deg  deg-to-rad-multiplier)) 

^Returns  first  n  elements  of  list. 

(defiin  near  (n  list) 

(cond  ((zerop  n)  nil) 

(t  (cons  (car  list)  (near  (1-  n)  (edr  list)))))) 
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;  "vector.cl" 


;  A  vector  is  a  list  of  numerical  atoms. 

(defim  vector-add  (vector- 1  vector-2)  (mapear '+  vector- 1  vector-2)) 

(defim  vector-subtract  (vector-1  vector-2)  (mapear  ’-  vcctor-1  vector-2)) 

(defim  scalar-multiply  (scalar  vector) 

(cond  ((null  vector)  nil) 

(t  (cons  (♦  scalar  (car  vector)) 

(scalar-multiply  scalar  (edr  vector)))))) 

(defim  dot-product  (.x  y) 

(apply '+  (mapear  '*  x  y)))  ;A  matrix  is  a  list  of  row  vectors. 

(defim  cross-product  (x  y)  ;x  and  y  are  3D  vectors. 

(list  (-  (*  (cadr  x)  (caddr  y))  (*  (cad^  x)  (cadr  y))) 

(-  (*  (caddr  x)  (car  y))  (•  (car  x)  (caddr  y))) 

(-  (*  (car  x)  (cadr  y))  (*  (cadr  x)  (car  y))))) 

(defim  vector-length  (vector)  (sqrt  (dot-product  vector  vector))) 

(defim  distance-between  (x  y)  '.points  x  and  y  represented  by  vectors, 
(vector-length  (vector-subtract  x  y))) 

;  returns  a  vector  (O*(one-position  -1)1  O*(length-one-position)) 
(defim  unit-vector  (one-position  length) 

(do  ((n  length  (1-  n)) 

(vector  nil  (cons  (cond  ((=  one-position  n)  1)  (t  0))  vector))) 
((zerop  n)  vector))) 

(defim  appendl  (L)  (append  L  '(1))) 
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;  ''niatrix.d'' 


;  requires  VECTOR.  CL 
;  requires  MISC.CL  "near" 

;  A  matrix  is  a  list  of  row  vectors. 

(defim  uanspose  (A) 

(cond  ((null  (edr  A))  (mapear  'list  (car  A))) 

(t  (mapear  'cons  (car  A)  (transpose  (edr  A)))))) 

(defun  post-multiply  (M  x)  ;M  is  a  square  matri.x.  x  is  a  conformable  vector, 
(cond  ((null  (edr  I^)  (list  (dot-product  (car  M)  x))) 

(t  (cons  (dot-product  (car  M)  x)  (post-multiply  (edr  M)  x))))) 

(defim  pre-multiply  (vector  matrix) 

(post-multiply  (transpose  matrix)  vector)) 

(defim  matrix-multiply  (A  B)  ;A  and  B  are  conformable  matrices. 

(cond  ((null  (edr  A))  (list  (pre-multiply  (car  A)  B))) 

(t  (cons  (pre-multiply  (car  A)  B)  (matrix-multiply  (edr  A)  B))))) 

(defim  chain-multiply  (L)  ;L  is  a  list  of  names  of  conformable  matrices, 
(cond  ((null  (eddr  L))  (matrix-multiply  (eval  (car  L))  (eval  (cadr  L)))) 

(t  (matrix-multiply  (eval  (car  L))  (chain-multiply  (edr  L)))))) 

(defim  cycle-left  (matrix)  (mapear  'row-cycle-left  matrix)) 

(defim  row-cycle-left  (row)  (append  (edr  row)  (list  (car  row)))) 

(defim  cycle-up  (matrix)  (append  (edr  matrix)  (list  (car  matrix)))) 

(defim  unit-matrix  (size) 

(do  ((row-number  size  (1-  row-number)) 

(I  nil  (cons  (unit-vector  row-number  size)  I))) 

((zerop  row-number)  I))) 

(defim  concat-matrix  (A  B)  ;A  and  B  are  matrices  with  equal  number  of  rows, 
(cond  ((null  A)  B) 

(t  (cons  (append  (car  A)  (car  B))  (concat-matrix  (edr  A)  (edr  B)))))) 

(defim  augment  (matrix) 

(concat-matrix  matrix  (unit-matrix  (length  matrix)))) 

(defim  normalize-row  (row)  (scalar-multiply  (/  1.0  (car  row))  row)) 
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(defim  solve-first-column  (matrix)  .Reduces  first  column  to  (1  0  ...  0). 

(do*  ((LI  matrix  (cdr  LI)) 

(L2  (nonnalize<row  (car  matrix))) 

(L3  (list  L2)  (cons  (vector-add  (car  LI) 

(scalar-multiply  (-  (caar  LI))  L2))  L3))) 

((null  (cdr  LI))  (reverse  L3)))) 

(defun  square-car  (M)  ;Retums  square  matrix  extracted  from  front  of  matrix  M. 
(do  ((m  (length  M)) 

(LI  M  (cdr  LI)) 

(L2  nil  (cons  (near  m  (car  LI))  L2))) 

((null  LI)  (reverse  L2)))) 

::L  is  a  list  of  lists.  This  function  finds  list  with 
largest  car  and  moves  it  to  head  of  list  of  lists. 

(defim  max-car-first  (L) 

(cond  ((null  (cdr  L))  L) 

(t  (if  (>  (abs  (caar  L))  (abs  (caar  (max-car-first  (cdr  L)))))  L 
(append  (max-car-first  (cdr  L))  (list  (car  L))))))) 

;  Applies  max-car-first  to  first  n  elements  of  list. 

(defim  nmax-car-first  (n  list) 

(append  (max-car-first  (near  n  list))  (nthedr  n  list))) 

(defim  matrix-inverse  (M) 

(do  ((Ml  (max-car-first  (augment  M)) 

(cond  ((null  Ml)  nil) 

(t  (nmax-car-first  n  (cycle-left  (cycle-up  Ml)))))) 

(n(l-  (length  M))(l-n))) 

((or  (minusp  n)  (null  Ml))  (cond  ((null  Ml)  nil)  (t  (square-car  Ml)))) 

(setq  Ml  (cond  ((zerop  (caar  Ml))  nil)  (t  (solve-first-column  Ml)))))) 
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;  *'kinematics.cr' 


;  requires  MATRIX.  CL 

(defim  dh-mathx  (cosrotate  sinrotate  costwist  sinmist  length  translate) 
(list  (list  cosrotate  (-  (*  costwist  sinrotate)) 

(*  sintwist  sinrotate)  (*  length  cosrotate)) 

(list  sinrotate  (*  costwist  cosrotate) 

(-  (*  sintwist  cosrotate))  (*  length  sinrotate)) 

(list  0.  sintwist  costwist  translate)  (list  0.  0.  0.  1.))) 

(defiin  mdh-matrix  (cosrotate  sinrotate 
costwist-i-1  sintwist-i-1 
length-i-1  translate) 

(list  (list  cosrotate  (-  sinrotate)  0.  length-i-1) 

(list  (•  sinrotate  costwist-i-1)  (♦  cosrotate  costwist-i-1) 

(-  sintwist-i-1)  (-  (*  sintwist-i-1  translate))) 

(list  (*  sinrotate  sintwist-i-1)  (♦  cosrotate  sintwist-i-1) 
costwist-i-1  (*  costwist-i-l  translate)) 

(listO.  0.  0.  1.))) 

(defiin  homogeneous-transform  (azimuth  elevation  roll  x  y  z) 
(rotation-and-translation  (sin  azimuth)  (cos  azimuth)  (sin  elevation) 
(cos  elevation)  (sin  roll)  (cos  roll)  x  y  z)) 

(defim  rotation-and-translation  (spsi  cpsi  sth  cth  sphi  cphi  x  y  z) 

(list  (list  (*  cpsi  cth)  (-  (*  cpsi  sth  sphi)  (•  spsi  cphi)) 

(+  (*  cpsi  sth  cphi)  (*  spsi  sphi))  x) 

(list  (*  spsi  cth)  (+  (*  cpsi  cphi)  (•  spsi  sth  sphi)) 

(-  (♦  spsi  sth  cphi)  (*  cpsi  sphi))  y) 

(list  (-  sth)  (*  cth  sphi)  (*  cth  cphi)  z) 

(list  0.  0.  0.  1.))) 

(defim  inverse-H  (H) 

(let*  ((minus-P  (list  (-  (fourth  (first  H))) 

(-  (fourth  (second  H))) 

(-(fourth  (third  H))))) 

(inverse-R  (transpose  (square-car  (reverse  (rest  (reverse  H)))))) 
(inverse-P  (post-multiply  inverse-R  minus-P))) 

(append  (concat-mathx  inverse-R  (transpose  (list  inverse-P))) 
(list  (list  0  0  0  1))))) 
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;  "rigid-body.cl” 


;  requires  KINEMATICS.CL 
(defclass  rigid-borb'  0 

((location  ;The  three-vector  (x  y  z)  in  world  coordinates. 

;initarg :  location 
:accessor  location) 

(velocity  ;The  six-vector  (u  v  w  p  q  r)  in  body  coordinates. 

:initarg : velocity 
:accessor  velocity) 

(acceleration  ;The  vector  (u-dot  v-dot  w-dot  p-dot  q-dot  r-dot). 

:  accessor  acceleration) 

(forces-and-torques  ;The  vector  (Fx  Fy  Fz  L  M  N)  in  body  coordinates, 
laccessor  forces-and-torques) 

(moments-of-inertia  ;The  vector  (Ix  ly  Iz)  in  principal  axis  coordinates. 
:initarg ;  moments-of-inertia 
;accessor  moments-of-inertia) 

(mass 

:initarg  ;mass 
:accessor  mass) 

(node-list  ;List  of  vertices  for  wire  frame  model 
;initarg  ;node-list 
;accessor  node-list) 

(polygon-list  ;Sets  of  above  vertices  defining  polygons 

initarg  ;polygon-list  ;Ex:'((l  2  3)(2  3  4  5X4  5  6)) 

:accessor  polygon-list) 

(transformed-node-list 
:accessor  transformed-node-list) 

(H-matrix 
:accessor  H-matrix) 

(cunent-time 
:accessor  current-time))) 

(defmethod  move  ((body  rigid-body)  azimuth  elevation  roll  x  y  z) 

(setf  (H-matrix  body) 

(homogeneous-transform  azimuth  elevation  roll  x  y  z)) 
(transform-node-list  body) 

(update-position  body)) 
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[ 


(definethod  inove*incremental  ((body  rigid*body)  increment-list) 
(setf  (H-matrix  body) 

(mathx-multiply  (H-matrix  body)  (homogeneous-transform 
(first  increment-list) 

(second  increment-list) 

(third  increment-list) 

(fourth  increment-list) 

(fifth  increment-list) 

(sixth  increment-list)))) 
(transform-node-list  body) 

(update-position  body)) 


(defmethod  get-delta-t  ((body  rigid-body)) 

(let*  ((new-time  (get-intemal-real-time)) 

(delta-t  (/  (-  new-time  (current-time  body))  1000))) 

(setf  (cunent-time  body)  ne>v-time) 
delta-t)) 

(defmethod  start-timer  ((body  rigid-body)) 

(setf  (cunent-time  body)  (get-intemal-real-time))) 

(defmethod  update-rigid-body  ((body  rigid-body))  ;Euler  integration, 
(let*  ((delta-t  (get-delta-t  body))) 

(update-H-matrix  body  delta-t) 

(transform-node-list  body) 

(update-position  body) 

(update-velocity  body  deita-t) 

(update-acceleration  body))) 

(defmethod  update-acceleration  ((body  rigid-body)) 

(setf  (acceleration  body)  ;(list  u-dot  v-dot  w-dot  p-dot  q-dot  r-dot) 
(multiple-value-bind  .Assumes  principal  axis 

(Fx  Fy  Fz  L  M  N  u  v  w  p  q  r  Ix  ly  Iz)  ;cootdit]iates  with  origin  at 
(values-list  ;center  of  gravity  of  botfy. 

(append 

(forces-and-torques  body)  (velocity  body)  (moments-of-inertia  body))) 
(list  (+  (•  V  r)  (*  -1  w  q)  (/  Fx  (mass  body)) 

(*  *gravity*  (first  (third  (H-matrix  body))))) 

(+  (*  w  p)  (*  -1  u  r)  (/  Fy  (mass  body)) 

(*  *gTavity*  (secotid  (third  (H-matrix  body))))) 

(+  (*  u  q)  (•  -1  V  p)  (/  Fz  (mass  body)) 

(*  *gravity*  (thi^  (third  (H-matrix  body))))) 

(/  (+  (•  (-  ly  Iz)  q  r)  L)  Ix) 

(/(+(*(-IzIx)Tp)M)ly) 

(/(+(*(-IxIy)pq)N)Iz))))) 
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(defmethod  update-velocity  ((body  rigid-body)  delu-t) 

(setf  (velocity  body) 

(vector-add  (velocity  body) 

(scalar-multiply  delta-t  (acceleration  body))))) 

(definethod  update-H-matrix  ((body  rigid-body)  delta-t) 

(setf  (H-matrix  body) 

(matrix-multiply 

(H-matrix  body)  (homogeneous-transform 
(*  delta-t  (sixth  (velocity  bod>’))) 

(*  delta-t  (fifth  (velocity  body))) 

(*  delta-t  (fourth  (velocity  body))) 

(*  delta-t  (first  (velocity  body))) 

(*  delta-t  (second  (velocity  body))) 

(•  delta-t  (third  (velocity  body))))))) 

(definethod  transform-node-list  ((body  rigid-body)) 

(setf  (transformed-node-list  body) 

(mapcar  #'(latnbda  (node-location) 

(post-multipiy  (H-matrix  body)  node-location)) 

(node-list  body)))) 

(defmethod  update-position  ((body  rigid-body)) 

(setf  (location  body)  (near  3  (first  (transformed-node-list  body))))) 

;  (defconstant  •gravity*  32.2185) 

(definethod  world-to-body  ((body  rigid-body)  xyz-pos) 

(near  3  (post-multiply  (inverse-H  (H-matrix  body)) 

(append  xyz-pos '(!))))) 

(definethod  body-to-world  ((body  rigid-body)  xyz-pos) 

(near  3  (post-multiply  (H-matrix  body)  (append  xyz-pos '(!))))) 
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;  "strobc-cameni.cr' 


;  requires  RIGID-BODY.  CL 
(require  :xcw) 

(cw;  initialize-common-windows) 

(defclass  strobe-camera  (rigid-body  ) 

((focal-length 
accessor  focal-length 
linitform  6) 

(camera-window 
:accessor  camera-window 
initform  (cw;  make-window-stream  ;borders  5 
:left  500 
:bottom  500 
:width  300 
;  height  300 

;title  "strobe-camera-image'* 

:activate-p  t)) 

(H-matrix 

:initform  (homogeneous-transform  .3  -.3  0  -300  -90  -90)) 

(inverse-H-matrix 
;accessor  inverse-H-matrix 

linitform  (inverse-H  (homogeneous-transform  .3  -.3  0  -300  -90  -90))) 
(enlargement-l^or 
:accessor  enlargement-factor 
;initform  30))) 

(definethod  erase-camera-window  ((camera  strobe-camera)) 

(cw:clear  (camera-window  camera))) 

(defmethod  move  ((camera  strobe-camera)  azimuth  elevation  toll  x  y  z) 

(setf  (H-matrix  camera)  (homogeneous-transform  azimuth  elevation  roll  x  y  z)) 
(setf  (inverse-H-matrix  camera)  (inverse-H  (H-matrix  camera)))) 

(defmethod  take-picture  ((camera  strobe-camera)  (body  rigid-botfy)) 

(let  ((camera-space-node-list  (nuqKar  #'(lambda  (node-location) 

(post-multiply  (inverse-H-matrix  camera)  node-location)) 
(transfonned-node-list  body)))) 

(dolist  (polygon  (polygon-list  body)) 

(clip-and-draw-polygon  camera  polygon  camera-space-node-list)))) 
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(defoiethod  clip>and-draw>polygon 
((camera  strobe-camera)  polygon  node-coord-list) 

(do*  ((initial-point  (nth  (first  polygon)  node-coord-list)) 

(firom-point  initial-point  to-point) 

(remaining-nodes  (rest  polygon)  (rest  remaining-nodes)) 

(to-point  (nth  (first  remaining-nodes)  node-coord-list) 

(if  (not  (null  (first  remaining-nodes))) 

(nth  (first  remaining-nodes)  node<oord-list)))) 

((null  to-point) 

(draw-clipped-projection  camera  from-point  initial-point)) 
(draw-clipped-projection  camera  from-point  to-point))) 

(defmethod  draw<lipped-projection  ((camera  strobe-camera)  from-point  to-point) 
(cond  ((and  (<=  (first  from-point)  (focal-length  camera)) 

(<=  (first  to-point)  (focal-length  camera)))  nil) 

((<=  (first  from-point)  (focal-length  camera)) 

(draw-line-in-camera-window  camera 

(perspective-transform  camera  (from-clip  camera  from-point  to-point)) 
(perspective-transform  camera  to-point))) 

((<=  (first  to-point)  (focal-length  camera)) 

(draw-line-in-camera-window  camera 
(perspective-transform  camera  from-point) 

(perspective-transform  camera  (to-clip  camera  from-point  to-point)))) 

(t  (draw-line-in-camera-window  camera 
(perspective-transform  camera  from-point) 

(perspective-transform  camera  to-point))))) 

(defmethod  from<lip  ((camera  strobe-camera)  from-point  to-point) 

(let  ((scale-factor  (/  (-  (focal-length  camera)  (first  fiom-point)) 

(-  (first  to-point)  (first  fi-om-point))))) 

(list  (+  (first  firom-point) 

(*  scale-factor  (-  (first  to-point)  (first  firom-point)))) 

(+  (second  from-point) 

(*  scale-factor  (-  (second  to-point)  (second  from-point)))) 

(+  (third  from-point) 

(*  scale-factor  (-  (third  to-point)  (third  from-point))))  1))) 

(defmethod  to-clip  ((camera  strobe-camera)  from-point  to-point) 

(from-clip  camera  to-point  firom-point)) 

(defmethod  draw-line-in-camera-window  ((camera  strobe-camera)  start  end) 
(cw;draw-line  (camera-window  camera) 

(cw.make-position  rx  (first  start)  ;y  (second  start)) 

(cw;make-position  ;x  (first  end)  ;y  (second  end)) 

;brush-width  0)) 


98 


(defmetbod  perspective-transform  ((camera  strobe<amera)  point-in-camera-space) 
(let*  ((enlmgement-£actor  (enlargement-foctor  camera)) 

(focal-length  (focal-length  camera)) 

(x  (first  point-in-camera-space))  ;x  axis  is  along  optical  axis 
(y  (second  point-in-camera-space))  ;y  is  out  right  side  of  camera 
(z  (third  point-in-camera-space)))  ;z  is  out  bottom  of  camera 
(list  (+  (round  (*  enlargement-factor  (/  (*  focal-length  y)  x))) 

150)  ;to  right  in  camera  window 

(+  150  (round  (*  enlargement-factor  (/  (*  focal-length  (-  z))  x)) 

)))))  ;up  in  camera  window 
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;  •Mink.cr’ 


;  requires  RIGID-BODY.  CL 

(defclass  link  (rigid-body) 
((motion-limit-flag 
:initform  nil 

;  accessor  motion-limit-flag) 
(mist-angle 
:initarg :  twist-angle 
:  accessor  mist-angle) 

(link-length 
initarg ;  link-length 
accessor  link-length) 
(inboard-joint-angle 
initarg  inboard-joint-angle 
;accessor  inboard-joint-angle) 
(inboard-joint-displacement 
initarg  inboard-joint-displacement 
:accessor  inboard-joint-displacement) 
(inboard-link 
initarg  inboard-link 
:accessor  inboard-link) 

(A-matrix 
:accessor  A-matrix) 

;  added  for  mdh 
(twist-angle-i-l 
initarg  ;twist-angle-i-l 
:accessor  twist-angle-i-l) 
(link-length-i-1 
initarg  ;link-length-i-i 
;accessor  link-length-i-l) 

(T-matrix 

:accessor  T-matrix))) 

(defclass  rotary-link  (link) 
((min-joint-angle 
initarg  :min-joint-angle 
laccessor  min-joint-angle) 
(max-joint-angle 
initarg  imax-joint-angle 
laccessor  max-joint-angle))) 


(defclass  sliding-link  (link) 
((min-joint-displ2Kement 
;initarg  ;nun-joint-dispiacement 
:accessor  min-joint-displacement) 
(max-joint-displacement 
:initarg  imax-joint-displacement 
:accessor  max-joint-displacement))) 


;  "aqua-data.cl'' 


;  load  after  MISC.CL 

(defconstant  *dt*  0.01)  ;  delta-t  each  update. 

(defconstant  *loops*  2)  ;  updates  between  draws. 

(defconstant  linkOlength  37.5) 

(defconstant  link  1  length  20.0) 

(defconstant  Unk21ength  52.0) 

(defconstant  link31ength  102.0) 

(defconstant  flag-length  25.0) 

;  leg  attachment  angles. 

(defconstant  leg  1 -angle  (deg-to-rad  0)) 

(defconstant  leg2-angle  (deg-to-rad  60)) 

(defconstant  leg3-angle  (deg-to-rad  120)) 

(defconstant  leg4-angle  (deg-to-rad  180)) 

(defconstant  legS-angle  (deg-to-rad  240)) 

(defconstant  leg6-angle  (deg-to-rad  300)) 

;  initial  position  and  orientation  in  world  coordinates. 

(defconstant  azimuth-init  (deg-to-rad  0.0)) 

(defconstant  elevation-init  (deg-to-rad  0.0)) 

(defconstant  roll-init  (deg-to-rad  9.0)) 

(defconstant  x-init  0.0) 

(defconstant  y-init  0.0) 

(defconstant  z-init  -135.0) 

;  initial  (default)  joint  angles. 

(defconstant  jointl-init  (deg-to-rad  0.0)) 

(defconstant joint2-init  (deg-to-rad  25.0)) 

(defconstant  joint3-init  (deg-to-rad  -1 15.0)) 

(defconstant  default-angles  (list  jointl-init  joint2-initjoint3-init)) 

;  joint  spring  constants,  (fill  in :  Kg-cin2/sec2  per  radian) 

(defconstant  joint  1-K  -2(K)0000) ;  5000000  =  Scott's  500  Nm  per  radian, 
(defconstant  joint2-K  -2000000) 

(defconstant  joint3-K  -2000000) 

(defconstant  spring-constants  (list  jointl-K  joint2-K  joint3-K)) 

;  joint  spring  damping  constants,  (fill  in ;  Kg-cm2/sec2  per  radian/sec) 
(defconstant  jointl-D  -800000)  ;  800000  =  Scott's  80  Nm-sec  per  radian/sec. 
(defconstant  joint2-D  -800000) 

(defconstant  joint3-D  -800(X)0) 

(defconstant  spring-damping-constants  (list  jointl-D  joint2-Djoint3-D)) 
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;  joint  limits. 

;(defconstant  joint l-min-limit  (deg-to-rad  -60.0)) 

'.(defconstant  joint  1 -max-limit  (deg-to-rad  60.0)) 

;(defconstant  joint2-min-limit  (deg-to-rad  -106.6)) 

;(defconstant  joint2-max-limit  (deg-to-rad  73.4)) 

'.(defconstant  joint3-min-limit  (deg-to-rad  -156.4)) 

;(defconstant  joint3-niax-limit  (deg-to-rad  23.6)) 

(defconstant  joint  1  -min-limit  -50.0) 

(defconstant  joint  1 -max-limit  50.0) 

(defconstant  joint2 -min-limit  -50.0) 

(defconstant  joint2 -max-limit  50.0) 

(defconstant  joint3 -min-limit  -50.0) 

(defconstant  joints -max-limit  50.0) 

;  mass  in  Kg. 

(defconstant  aqua-body-mass  500.0) 

^defconstant  linklmass  0.0) 

;(defconstant  link2mass  0.0) 

;(defconstant  link3mass  0.0) 

;  (I.X  ly  Iz)-Kg-cm.  in  principal  axis  coordinates. 

;  assumes  solid  cylindrical  of  constant  density. 

(defconstant  aqua-body-height  50.0) 

(defconstant  aqua-body-radius  30.0) 

(defconstant  aqua-body-Ix 

(+  (*  (/ 1  4)  aqua-body-mass  (sqr  aqua-body-radius)) 

(•  (/ 1  12)  aqua-body-mass  (sqr  aqua-hody-height)))) 

(defconstant  aqua-body-ly  aqua-body-Ix) 

(defconstant  aqua-body-Iz  (•  (/ 1  2)  aqua-body-mass  (sqr  aqua-body-radius))) 
(defconstant  aqua-body-inertia  (list  aqua-body-Ix  aqua-body-Iy  aqua-body-Iz)) 

;  center  of  mass. 

(defconstant  body-mass-center  '(0  0  0)) 

;(defconstant  link  I  mass-center  (list  (/  linkllength  2)  0  0)) 

;(defconstant  link2mass-center  (list  (/  link2iength  2)  0  0)) 

;(defconstant  link3  mass-center  (list  (/  link3  length  2)  0  0)) 

(defconstant  ^gravity*  980.0)  ;cm/sec/sec. 
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;  ''aqua-link.cr' 


;  requires  LINK.CL 
.  requires  AQUA-DATA.CL 

(defclass  linkO  (rotary-link) 

((twist-angle  linitform  0) 

(link-length  :initform  linkOlength) 

(inboard-joint-angle  :initfonn  0) 
(inboard-joint-displacement  :initform  0) 
(min-joint-angle  ;initform  (deg-to-rad  -360)) 
(max-joint-angle  linitform  (deg-to-rad  360)) 

(node-list  :initform  (list  (list  0  0  0  1)  (list  0  0  0  1) 

(list  linkOlength  0  0  1)))  :  for  mdh 

;  (list  (-  linkOlength)  0  0  1))) :  for  dh 

(polygon-list  :initform  ’((I  2))))) 

(defclass  linkl  (rotary-link) 

((twist-angle  initform  (deg-to-rad  -90)) 

(link-length  :initform  link  (length) 
(inboard-joint-angle  :  initform  joint  1-init) 
(inboard-joint-displacement  ;initform  0) 
(min-joint-angle  ;initformjointl-min-limit) 
(max-joint-angle  linitform  joint  1  -max-limit) 

(node-list  ;initform  (list  (list  0  0  0  1)  (list  0  0  0  1) 

(list  linkllength  0  0  1)))  ;  for  mdh 

;  (list  (-  linkllength)  0  0  1))) ;  for  dh 

(polygon-list :  initform  '((1  2))))) 

(defclass  link2  (rotary-link) 

((twist-angle ;  initform  0) 

(link-length  rinitform  link21ength) 
(inboard-joint-angle  ;initform  joint2-init) 
(inboard-joint-displacement  :initform  0) 
(min-joint-angle :  initform  joint2-min-limit) 
(max-joint-angle  .initform  joint2-max-limit) 

(node-list  linitform  (list  (list  0  0  0  1)  (list  0  0  0  1) 

(list  link21ength  0  0  1)))  ;  for  mdh 

;  (list  (-  link21ength)  0  0  1))) ;  for  dh 

(polygon-list  linitfoim  '((1  2))))) 
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(defciass  link3  (rotaiy-link) 

((twist-angle  ;initfonn  0) 

(link-length  :initform  link31ength) 

(inboard-joint-angle  ;initfonnjoint3-init) 
(inboard-joint-displacement  linitform  0) 

(min-joint-angle  ;initformjoint3-nun-linut) 

(max-joint-angle  linitform  joint3-max-limit) 

(node-list  :initform  (list  (list  0  0  0  1)  (list  0  0  0  1) 

(list  link3 length  0  0  1)))  ;  for  mdh 

(list  (-  link3length)  0  0  1))) :  for  dh 
(polygon-list  :initform  '((1  2))))) 

for  dh 

(defmethod  update-A-matrix  ((link  link)) 

(with-slots  (twist-angle  link-length  inboard-joint-angle 
inboard-joint-displacement  A-matrix)  link 
(setf  A-matrix 

(dh-matrix  (cos  inboard-joint-angle)  (sin  inboard-joint-angle) 
(cos  twist-angle)  (sin  twist-angle) 
link-length  inboard-joint-displacement)))) 

;  added  for  mdh 

(defmethod  update-T-matrix  ((link  link)) 

(with-slots  (twist-angle-i-1  link-length-i-1  inboard-joint-angle 
inboard-joint-displacement  T-matrix)  link 
(setfT-matrix 

(mdh-matrix  (cos  inboard-joint-angle)  (sin  inboard-joint-angle) 
(cos  twist-angle-i-1)  (sin  twist-angle-i-1) 
link-length-i-1  inboard-joint-displacement)))) 

(defmethod  rotate  ((link  link)  angle) 

(setf  (inboard-joint-angle  link)  angle) 

(update-T-matrix  link) 

(setf  (H-matrix  link)  (matrix-multiply  (H-matrix  (inboard-link  link)) 
(T-matrix  link))) 

(transform-node-list  link)) 

(defmethod  rotate-link  ((link  link)  angle) 

(cond  ((>  angle  (max-joint-angle  link)) 

(rotate  link  (max-joint-angle  link)) 

(setf  (motion-limit-flag  link)  t)) 

((<  angle  (min-joint-angle  link)) 

(rotate  link  (min-joint-angle  link)) 

(setf  (motion-limit-flag  link)  t)) 

(t  (rotate  link  angle)  (setf  (motion-limit-flag  link)  nil)))) 
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;  '’aqua.d" 


;  requires  STROBE-CAMERA.CL 
;  requires  AQUA-LEG.CL 

(defclass  aquarobot-body  (rigid-body) 

((mass  linitformaqua-body-mass) 

(moments-of-inertia  linitform  aqua-body-inertia) 

(node-list 

linitform  (list  (list  0  0  0  1) 

(list  (*  (cos  legl-angle)  linkOlength) 

(*  (sin  legl-angle)  linkOlength)  0  1) 

(list  (*  (cos  leg2 -angle)  linkOlength) 

(*  (sin  leg2-angle)  linkOlength)  0  1) 

(list  (*  (cos  leg3-angle)  linkOlength) 

(*  (sin  Ieg3-angle)  linkOlength)  0  1) 

(list  (*  (cos  leg4-angle)  linkOlength) 

(*  (sin  leg4-angle)  linkOlength)  0  1) 

(list  (*  (cos  leg5-angle)  linkOlength) 

(*  (sin  legS-angle)  linkOlength)  0  1) 

(list  (*  (cos  leg6-angle)  linkOlength) 

(*  (sin  ieg6-angle)  linkOlength)  0  1) 

(list  linkOlength  0  (-  flag-length)  1))) 

(polygon-list 

linitform  ’((I  2  3  4  5  6)  (1  7))))) 

(defclass  aquarobot  () 

((body 

linitform  (make-instance  'aquarobot-body) 
laccessor  body) 

(legl 

linitform  (make-instance  'aqua-leg  ileg-attachment-angle  legl-angle) 
laccessor  legl) 

(Ieg2 

linitform  (make-instance  'aqua-leg  :leg-attachment-angle  leg2-angle) 
laccessor  leg2) 

(leg3 

linitform  (make-instance  'aqua-leg  :leg-attachment-angle  leg3-angle) 
laccessor  leg3) 

(leg4 

linitform  (make-instance  'aqua-leg  ileg-attachment-angle  Ieg4-angle) 
laccessor  leg4) 

(legs 

linitform  (make-instance  'aqua-leg  ileg-attachment-angle  legS-angle) 
laccessor  legS) 

(leg6 

linitform  (make-instance  'aqua-leg  ileg-attachment-angle  leg6-angle) 
laccessor  leg6))) 
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(definethod  worid*to-aqua  ((aqua  aquarobot)  xyz-pos) 
(world*to>body  (body  aqua)  xyz-pos)) 

(definethod  aqua-to>worId  ((aqua  aquarobot)  xyz-pos) 
(body-to-world  (body  aqua)  xyz-pos)) 

;(defmethod  initialize  ((aqua  aquarobot)) 

(self  (H-matrix  (body  aqua)) 

(homogeneous-transform  azimuth-init  elevation-init  roll-init 
x-init  y-init  z-init)) 

(transform-node-list  (body  aqua)) 

(update-position  (body  aqua)) 

(self  (forces-and-torques  (body  aqua))  '(00000  0)) 

(self  (acceleration  (b^y  aqua))  ’(00000  0)) 

(setf  (velocity  (body  aqua))  '(00000  0)) 

(start-timer  (body  aqua)) 

(initialize-leg  (legl  aqua)  (body  aqua)) 

(initialize-leg  (leg2  aqua)  (body  aqua)) 

(initialize-leg  (leg3  aqua)  (body  aqua)) 

(initialize-leg  (leg4  aqua)  (body  aqua)) 

(initialize-leg  (leg5  aqua)  (body  aqua)) 

(initialize-leg  (leg6  aqua)  (body  aqua))) 

(defim  aqua-picture  () 

(setf  aqua- 1  (make-instance  ’aquarobot)) 

(initialize  aqua-1) 

(move-incremental  aqua-1  null-move-list);sets  "prev-foot-pos". 
(setf  camera- 1  (make-instance  ’strobe-camera)) 

(take-picture  camera-1  aqua-1)) 

(definethod  take-picture  ((camera  strobe-camera)  (aqua  aquarobot)) 
(take-picture  camera  (body  aqua)) 

(take-picture  camera  (legl  aqua)) 

(take-picture  camera  (legl  aqua)) 

(take-picture  camera  (Ieg3  aqua)) 

(take-picture  camera  (leg4  aqua)) 

(take-picture  camera  (legS  aqua)) 

(take-picture  camera  (leg6  aqua))) 

(defim  new-picture  Q 
(erase-camera-window  canwta-1) 

(take-picture  camera-1  aqua-1)) 
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(defmethod  move-incremental  ((aqua  aquarobot)  increment-list) 
(move-incremental  (body  aqua)  (first  increment-list)) 
(move-incremental  (leg!  aqua)  (second  increment-list)) 
(move-incremental  (Ieg2  aqua)  (third  increment-list)) 
(move-incremental  (Ieg3  aqua)  (fourth  increment-list)) 
(move-incremental  (leg4  aqua)  (fifth  increment-list)) 
(move-incremental  (Ieg3  aqua)  (sixth  increment-list)) 
(move-incremental  (leg6  aqua)  (seventh  increment-list))) 

(defconstant  null-move-list  '((0  0  0  0  0  0)  (0  0  0)  (0  0  0)  (0  0  0) 

(0  0  0)  (0  0  0)  (0  0  0))) 

(defmethod  feasible-movep  ((aqua  aquarobot)  allowable-sinkage 
allowable-slippage) 

(and  (feasible-movep  (legl  aqua)  allowable-sinkage  allowable-slippage) 
(feasible-movep  (leg2  aqua)  allowable-sinkage  allowable-slippage) 
(feasible-movep  (leg3  aqua)  allowable-sinkage  allowable-slippage) 
(feasible-movep  (Ieg4  aqua)  allowable-sinkage  allowable-slippage) 
(feasible-movep  (legS  aqua)  allowable-sinkage  allowable-slippage) 
(feasible-movep  (Ieg6  aqua)  allowable-sinkage  allowable-slippage))) 

(defun  restart-aqua  () 

(initialize  aqua-1) 

(move-incremental  aqua-1  null-move-list);sets  "prev-foot-pos”. 
(new-picture)) 

;replace  some  rigid-body  functions; 

(defmethod  start-timer  ((body  aquarobot-body)) 

(setf  (current-time  body)  0)) 

(defmethod  get-delta-t  ((body  aquarobot-body)) 

(let*  ((delta-t  *dt*) 

(new-time  (+  (current-time  body)  delta-t))) 

(setf  (current-time  bot^)  new-time) 
delta-t)) 

(defmethod  update-aquarobot  ((aqua  aquarobot))  ;Eu)er  integration, 
(let*  ((body  (body  aqua)) 

(delta-t  (get-delta-t  body))) 

(update-acceleration  body) 

(update-velocity  body  delta-t) 

(update-H-matrix  boify  delta-t) 

(tiansform-node-list  body) 

(update-position  body) 

(update-forces-and-torques  aqua)))  ;updates  positions  of  legs 
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;  ''aqua-lcg.cl' 


;  requires  AQUA.  CL 
.  requires  AQUA-LINK.CL 
;  requires  STROBE*CAMERA.CL 

(defclass  aqua-leg  () 

(( ieg-attachment-angle 
iinitarg :  Ieg-attachment-angle 
;  accessor  leg-attachment-angle) 
(linkO 

:initform  (make-instance  'linkO) 
:accessor  linkO) 

(linkl 

linitform  (make-instance  'linkl) 
;accessor  linkl) 

(link2 

.initform  (make-instance  'link!) 
:accessor  link2) 

(Iink3 

'.initform  (make-instance  'link3) 
accessor  link3) 
(motion-complete-flag 
:initform  nil 

:accessor  motion-complete-flag) 
(previous-foot-position 
;  initform  nil 

laccessor  previous-foot-position) 
(ciurent-foot'position 
;  initform  nil 

-.accessor  cunent-foot-position) 
(foot-contaa 
:initform  nil 
laccessor  foot-contact))) 
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(definethod  initialize-ieg  ((leg  aqua-leg)  (body  aquarobot-body)) 
(self  (foot-oHdact  leg)  nil) 

(self  (inboard-link  (linkO  leg))  body) 

(self  (inboard-link  (linkl  leg))  (linkO  leg)) 

(self  (inboard-link  (link2  leg))  (linkl  leg)) 

(self  (inboard-link  (link3  leg))  (link2  leg)) 

;  added  for  mdh 

(self  (twist-angle-i-l  (linkO  leg))  0) 

(self  (twist-angle-i-l  (linkl  leg))  (twist-angle  (linkO  leg))) 

(self  (twist-angie-i-1  (link2  leg))  (twist-angle  (linkl  leg))) 

(setf  (twist-angle-i-l  (link3  leg))  (twist-angle  (Iink2  leg))) 

(setf  (link-length-i-1  (linkO  leg))  0) 

(setf  (link-length-i-1  (linkl  leg))  (link-length  (linkO  leg))) 

(setf  (link-length-i-l  (linkl  leg))  (link-length  (linkl  leg))) 

(setf  (link-length-i-1  (link3  leg))  (link-length  (linkl  leg))) 
(set-default-angles  leg)) 

(definethod  set-default-angles  ((leg  aqua-leg)) 

(rotate-link  (linkO  leg)  (leg-attaclunent-angle  leg)) 

(rotate-link  (linkl  leg)  jointl-init) 

(rotate-link  (linkl  leg)  jointl-init) 

(rotate-link  (link3  leg)  joint3-init) 

(setf  (previous-foot-position  leg)  nil) 

(setf  (curitnt-foot-position  leg) 

(near  3  (third  (transfonned-node-list  (link3  leg)))))) ;  for  mdh 

(definethod  set-angles  ((leg  aqua4eg)  angle-list) 

(rotate-link  (linkO  leg)  (leg-attachment-angle  leg)) 

(rotate-link  (linkl  leg)  (car  angle-list)) 

(rotate-link  (linkl  leg)  (cadr  angle-list)) 

(rctate-link  (link3  leg)  (caddr  angle-list))) 

(definethod  take-picture  ((camera  strobe-camera)  (leg  aqua-leg)) 
(take-picture  camera  (linkl  leg)) 

(take-picture  camera  (linkl  leg)) 

(take-picture  camera  (link3  leg))) 
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(definethod  move'iiicremental  ((leg  aqua-teg)  increment-list) 

(rotate-link  (linkO  leg)  (leg-attachment-angle  leg)) 

(rotate-link  (linkl  leg) 

(+  (first  increment-list)  (inboard-joint-angle  (linkl  leg)))) 

(rotate-link  (link2  leg) 

(+  (second  increment-list)  (inboard-joint-angle  (Iink2  leg)))) 
(rotate-link  (Iink3  leg) 

(-•-  (third  increment-list)  (inboard-Joirt-angle  (Iink3  leg)))) 

(setf  (previous-foot-position  leg)  (current-foot-position  leg)) 

(setf  (current-foot-position  leg) 

(near  3  (third  (transformed-node-list  (link3  leg))))) :  for  mdh 
(near  3  (first  (transformed-node-list  (link3  leg))))) ;  for  dh 
(setf  (motion-complete-flag  leg)  (not  (or  (motion-limit-flag  (linkl  leg)) 
(motion-limit-flag  (Iink2  leg))  (motion-limit-flag  (tink3  leg)))))) 

(definethod  feasible-movep  ((leg  aqua-leg)  allowable-sinkage  allowable-slippage) 
(and  (<=  (third  (current-foot-position  leg))  allowable-sinkage) 

(or  (minusp  (third  (cunent-foot-position  leg))) 

(minusp  (third  (previous-foot-position  leg))) 

(<=  (vector-length  (vector-slippage  leg))  allowable-slippage)))) 

(definethod  vector-slippage  ((leg  aqua-leg)) 

(vector-subtract  (rest  (reverse  (previous-foot-position  leg))) 

(rest  (reverse  (current-foot-position  leg))))) 

(definethod  current-joint-angles  ((leg  aqua-leg)) 

(list  (inboard-joint-angle  (linkl  leg)) 

(inboard-joint-angle  (link!  leg)) 

(inboard-joint-angle  (Iink3  leg)))) 
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;  "aqu«>iiivcrse-ldacmatics.cl'' 


;  load  after  AQUA-LEG.CL 
;  load  after  AQUA-DATA.CL 

(defconstant  L2sqr  (sqr  link21ength)) 

(defconstant  L3sqr  (sqr  Iink3  length)) 

:  assumptions:  dh  coord  system  for  linkO  of  respective  leg: 
origin  at  joint  1, 

x-axis  directed  away  from  center  of  body, 
z-axis  aligned  with  body  z-axis; 

:  foot-position  =  '(x  y  z). 

(defun  theta  1  (foot-position) 

(if  (<  (car  foot-position)  0) 

(atanl  (-  (car  foot-position))  (-  (cadr  foot-position))) 

(atan2  (car  foot-position)  (cadr  foot-position)))) 

:  assumptions:  dh  coord  system  for  linkl  of  respective  leg; 
origin  at  joint2, 

;  x-axis  directed  away  from  joint  I, 

z-axis  aligned  with  body  z-axis; 

;  foot-position  =  '(x  y  z); 

;  hyp  =  distance  from  joint2  to  foot. 

(defun  thet^  (foot-position  hyp  hyp-sqr) 

(-  (acos  (/  (+  L2sqr  hyp-sqr  (-  L3sqr))  (•  2  link21eogth  hyp))) 

(if  (<  (car  foot-position)  0) 

(-  pi  (asin  (/  (caddr  foot-position)  hyp))) 

(asin  (/  (caddr  foot-position)  hyp)))^ 

;  assumptions;  same  as  for  theta2. 

(defim  theta3  (foot-position  hyp-sqr) 

(-  (acos  (/  (+  L2sqr  L3sqr  (-  hyp-sqr))  (*  2  link21ength  Unk3length)))  pi)) 


;  returns  foot  position  with  respect  to  joint  1  in  linkO  coord. 

(deftnethod  foot-joint  lAinkOcoord  ((leg  aqua-leg)  foot-pos) 
(vector-subtract  (world-to-body  (linkO  leg)  foot-pos) 

(list  linkOlength  0  0))) 

;  returns  foot  position  with  respect  to  joint  2  in  linkl  coord. 

;  given  foot-joint  l/linkOcoord. 

(defun  foot-joint2/Iinklcootd  (foot-pos) 

(list  (-  (sqrt  (+  (sqr  (car  foot-iios))  (sqr  (cadr  foot-pos))))  linkl  length) 
0  (caddr  foot-pos))) 
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;  returns  list  of  joint  angles  required  for  given  (world  coord)  foot  position, 
(definethod  aqua-inv-kin  ((leg  aqua-leg)  foot-position) 

(let*  ((posO  (foot-jointl/linkOcoord  leg  foot-position)) 

(posl  (foot-joint2Ainklcooid  posO)) 

(hyp-sqr  (+  (sqr  (car  posl))  (sqr  (caddr  posl)))) 

(hyp  (sqrt  hyp-sqr))) 

(list  (theta  1  posO) 

(theta2  posl  hyp  hyp-sqr) 

(theu3  posl  hyp-sqr))))  • 


;  "aqua-jacobiafi.d'' 


(defmethod  Jacobian  ((leg  aqua-leg)) 

(let*  ((TOl  (+  (leg-attachment-angle  leg) 

(inboard-joint-angle  (linkl  leg)))) 

(SO I  (sin  TO D)  (CO  1  (cos TOl)) 

(T2  (inboard-joint-angle  (link2  leg))) 

(S2  (sinT2))  (C2(cosT2)) 

(T23  (+  T2  (inboard-joint-angle  (link3  leg)))) 

(S23  (sin  T23))  (C23  (cos  T23)) 

(LI  linkllength)  (L2  link21ength)  (L3  link31ength)) 

(list  (list  (-  (*  (+  LI  (*  L2  C2)  (*  L3  C23))  SOI)) 

(-  (*  (+  (*  L2  S2)  (*  L3  S23))  COl)) 

(-  (*  L3  COl  S23))) 

(list  (*  (+  LI  (*  L2  C2)  (*  L3  C23))  COl) 

(-  (*  (+  (*  L2  S2)  (*  L3  S23))S01)) 

(-  (*  L3  SOI  S23))) 

(list  0 

(-  (+  (*  L2  C2)  (*  L3  C23))) 

(-(*  L3  C23)))))) 

(defmethod  inverse-Jacobian  ((leg  aqua-leg)) 

(matrix-inverse  (Jacobian  leg))) 

(defmethod  foot-to-Joint-rates  ((leg  aqua-leg)  dX  dY  dZ) 
(post-multiply  (inverse-Jacobian  leg)  (list  dX  dY  dZ))) 

(defmethod  Joint-to-foot-rates  ((leg  aqua-leg)  dthetal  dtheta2  dtheta3) 
(post-multiply  (Jacobian  leg)  (list  dthetal  dtheta2  dtheta3))) 
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;  ''aquarobot-update-forces-and-torques.cr' 

;  load  after  AQUA-DATA.CL 
;  load  after  AQUA.CL 
;  load  after  AQUA-LEG.  CL 

(deftnethod  update-forces-and-torques  ((aqua  aquarobot)) 

(setf  (forces-and-torques  (body  aqua))  '(00000  0))  ;clear  last  cycle, 
(add-leg-forces-and-torques  (legl  aqua)) 

(add-leg-forces-aiid-torques  (leg2  aqua)) 

(add-leg-forces-and-torques  (leg3  aqua)) 

(add-leg-forces-and-torques  (leg4  aqua)) 

(add-leg-forces-and-torques  (leg5  aqua)) 

(add-leg-forces-and-torques  (leg6  aqua))) 

(deftnethod  add-leg-forces-and-torques  ((leg  aqua-leg)) 

(if  (or  (foot-contact  leg)  (new<ontact  leg)) 

(let*  ((body  (inboard-link  (linkO  leg))) 

(joint-angles  (aqua-inv-kin  leg  (current-foot-position  leg)))) 
(set-angles  leg  joint-angles) 

(let*  ((r  (world-to-body  body  (cunent-foot-position  leg))) 
(omega  (cdddr  (velocity  body))) 

(foot-velocity  ;  in  bo^  coordinates 
(vector-add 

(scalar-multiply  -  I  (near  3  (velocity  body))) 
(cross-product  r  omega))) 

(torques  (vector-add 
(mapear  '*  spring<onstants 
(vector-subtract  joint-angles  default-angles)) 

(mapear  '*  spring-damping-constants 
(post-multiply  (inverse-jacobian  leg)  foot-velocity)))) 
(resultant-force 
(scalar-multiply 
-1  (post-multiply 

(matrix-inverse  (transpose  (jacobian  leg))) 
torques)))) 

(if  (still-in-contact  leg  resultant-force  body) 
(add-forces-and-toiques-to-body 
body  r  resultant-force)))))) 

(deftnethod  add-forces-and-torques-to-body  ((botfy  aquarobot-body)  r  f) 
(let  ((torques  (cross-product  r  f))) 

(setf  (forces-and-torques  body) 

(vector-add  (forces-and-torques  body) 

(append  f  torques))))) 


;update  joint  angles  and  foot  position,  detect  foot  hitting  ground. 

(defmethod  new-contact  ((.ig  aqua-leg)) 

(move-incremental  leg  '(0  0  0)) 

(if  (>  (third  (current-foot-position  leg))  0) 

(setf  (third  (current-foot-position  leg))  0 
(foot-contact  leg)  t) 
nil)) 

^detect  loss  of  contact,  (positive/down  z  component  in  world  coord) 

.side  effect  of  reseting  leg  to  default  state  when  nil  is  returned. 

(defmethod  still-in-contact  ((leg  aqua-leg)  force/body-xyz 
(body  aquarobot-body)) 

(let  ((force/world-.xyz  (vector-subtract  (body-to-world  body  force/body-.\yz) 
(location  body)))) 

(if  (>  (third  force/world-xyz)  0) 

(and  (set-default-angles  leg)  (setf  (foot-contact  leg)  nil)) 
t))) 
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APPENDIX  D 


OPERATING  INSTRUCTIONS 


Call  “droptest"  with  zero  to  four  arguments. 

First  arg  Spring  Constant  (2..  15,  default  S) 

Second  arg  Damper  Constant(O.S..  IS.  default  S) 

Third  arg  Drop  Height  (0. .  100,  default  0)  cm 

Fourth  arg  Update  Time  Increment  (10.. 50.  default  50)  ms 


SOURCE  CODE  FILES 


//  file  "droptest.c" 


/* 

/•  droptest.c 

/• 

/*  performer  Aquarobot  model  with  "spring"  joints. 

/• 

/•  •/ 

#inciude  <stdlib.h> 

#include  <stdio.h> 

#include  <string.h> 

^include  <math.h> 

#include  <gl/device.h> 

/*  performer  */ 

#include  "pfh" 

/*  performer  aqua*robot  object  constructor  *! 

#inciude  "pf_aqua.h" 

I*  physical  aqua-robot  object  constructor  and  controls  *! 
#include  "aqua.H" 

static  void  OpenPipeline  (pfPipe  *p); 
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void 

main  (int  argc,  char  •argv[]) 

{ 

pfPipe  ‘p; 
pfChannel  *chan; 
pfScene  *scene; 
pfDCS  ♦robot  jwsition; 

pfGroup  •aqua_robot;  /•  graphics  object  (performer)  ♦/ 

pfDCS  •JointDCS(61[41; 

aquarobot  robot;  /♦  physical  object  ♦/ 

//  defaults  for  args 
float  spring  =  SPRING_K; 
float  damp  =  SPRING_D; 
float  height  =  O.Of; 

float  step  =  0.05f;  //  default  ~  real  time 
//  process  args 

if  (argc  >!){//  spring  constant 
//  first  arg:  -15,000,000  <=  spring  <=  -2,000.000 
spring  =  fabs((float)(atoi(argv{lj))); 
if  (spring  <  2.0f) 
spring  =  2.0f; 
else  if  (spring  >  IS.Of) 
spring  =  I5.0f; 
spring  *=  -lOOOOOO; 

} 

if  (argc  >  2)  { //  spring  damping  constant 
//  second  arg;  -1,500,000  <=  damp  <“  -  50,000 
damp  =  fabs((float)(atoi(argv[21))); 
if  (dmnp  <  0.5f) 
damp  =  0.5f; 
else  if  (damp  >  15.0f) 
damp  ==  15.0f; 
damp  *=  -lOOOOO; 

} 

if  (argc  >  3)  { 

//  third  arg:  0  <=  drop  height  <=  100 
height  =  fabs((float)(atoi(argv[3]))); 
if  (height  >  lOO.Of) 
height  =  lOO.Of; 

} 

if  (argc  >  4)  { 

//  fourth  arg:  10ms  <»  integration  time  step  <=  50ms 
step  =  fabs((float)(atoi(argvl4J)))/1000.0f; 
if  (step  <  0.0  If) 
step  =  0.0  If; 
else  if  (step  >  0.05f) 
step  =  0.05f; 

} 
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/*  1.  initialize  Peifonner  */ 
pflnitO; 

/*  2.  configure  MP  mode  and  start  parallel  processes  */ 
pfConfigO; 

/*  3.  load  scene  database  */ 
scene  =  pfNe\vScene(); 

robot_position  =  pfNewDCS(); 
pfAddChild(scene,  robot jwsition); 
aqua_robot  =  MakeAquaRobot(JointDCS): 
pfAddChild(robot_position,  aqua_robot); 

/*  S.  configure  and  open  full-screen  pipeline  *! 
p  =  pfGetPipefO); 

pflnitPipefp,  OpenPipeline);  /*  pfInitPipe(p,  NULL);  •/ 

/*  set  frames  per  second  ( if  step  =  .05  sec,  then  ~  real  time)  */ 
pfFrameRate(20.0f); 

/*  6.  get  and  configure  channel  *! 
chan  =  pfNewChan(p); 
pfChanScenefchan.  scene); 
pfChanNearFarfchan,  0.  If,  lOOO.Of); 
pfChanFOVfchan,  45.0f,  -l.Of); 

I*  zero  clock  (not  really  needed)  */ 
pfInitClockO; 

I*  initialize  robot  *! 

robot.  initialize(spring,  damp,  height); 

update JointDCS(robot,  JointDCS); 

/*  set  up  view  position  */ 
pfCoord  view; 

pfSetVec3(view.hpr,  O.Of,  30.0f,  ISO.Of); 
pfSetVec3(view.xyz,  O.Of.  -500.0f,  -350.0f); 
pfChanViewfchan,  view.xyz,  view.hpr); 
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/*  7.  create  rendering  loop  */ 

/*  simulate  for  30  seconds  */ 
int  t  =  0; 

while  (t  <  600)  //  ~  20  frames  per  second 

{ 

/*  Transfer  robot  data  to  graphics  object.  */ 
pfDCSMatrix(robotj)osition,  robot. body.  H_matrix);  /*  body  •/ 
update JointDCSfrobot,  JointDCS);  /*  joints  •/ 

/*  Go  to  sleep  till  next  frame  time  */ 
pfSyncO;  t-H-; 

/♦  initiate  cull/draw  for  this  frame  *! 
pfFrameO; 

pfDra\vChanStats(chan); 

/•  Move  robot  to  new  position.  */ 
robot.update  aquarobot(step); 

} 

/*  8.  terminate  parallel  processes  and  exit  */ 

pfE.xit(); 

exit(O); 

} 
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/• 

*  OpenPipelineO  **  create  a  pipeline;  setup  the  window  system, 

*  the  DUS  GL.  and  IRIS  Performer.  This  procedure  is  executed  in 

*  the  draw  process  (when  there  is  a  separate  draw  process). 

•/ 

static  void 

OpenPipeline  (pfPipe  *p) 

{ 

pfLight  *Sun; 

I*  negotiate  with  window*manager  *! 
foregroundO; 
prefix>sition(0, 600,0,600); 
winopen("Aqua  Drop"); 

/*  negotiate  with  GL  */ 
pflnitGfi((p); 

/*  create  a  light  source  •/ 

Sun  =  pfNewLight(pfGetSharedArena()); 
pfLightPos(Sun,  O.Of,  O.Of,  l.Of,  O.Of); 

/*  create  a  default  lighting  model  */ 

pfApplyLModel(pfNewLModel(pfGetSharedArenaO)); 

pfLightOn(Sun); 

} 
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//rilc"pf.aqua.h" 


/* 

*  pf_aqiia.h 

*  call  ''MakeAquaRobot'*  to  make  AquaRobot  performer  object. 

*  JointDCS[i|lj]  points  to  pfDCS  for  leg  i,  joint  j. 

*  where  j  =  0  is  the  shoulder  Joint,  and  j  =  3  attaches  the  foot. 

•/ 

■  #include  "pf  h" 
pfGroup* 

MakeAquaRobot(pfD«  N  *JointDCS(61(4I); 


//  file  '’pf_aqua.c” 

/• 

♦  pf_aqua.c 

*  ~ 

*  call  "MakeAquaRobot”  to  make  AquaRobot  peifomMr  object. 

* 

•/ 

#include  "pf_aqua.h" 

#include  ”aqua_link.H" 

/*  polygon  data  for  aquarobot  */ 

#include  "polybody.h" 

^include  "polyshoulder.h" 

#include  "polyupperleg.h” 

#include  "polylowerleg.h" 

#include  "polyfoot-h” 

/*  geostate  for  multiple  parts  */ 
static  pfGeoState  *robo^llow _gstate; 
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pfGeoSet* 

MakeBodyGSet(void) 

{ 

pfGeoSet  *gset; 
void  *aieiia; 
pfMaterial  *mtl; 

arena  >  pfGetSharedArenaO; 
gset  pfNewGSet(arena); 

/*  set  the  coordinate  and  normal  arrays  */ 

pfGSetAttr(gset.  PFGS_COORD3,  PFGS_PER_VERTEX.  bodycoords.  NULL); 
pfGSetAttr(gset,  PFGSInORMAL3,  PFCfs_PER_PRIM,  bodynorms,  NULL); 

pfGSetPrimType(gset.  PFGS_QUADS); 
pfGSetNumPhmsCgset,  94); 

/*  set  up  geostate  for  "robotyellow"  material  */ 
robotyellow_gstate  «  pfNewGState(arena); 
mtl  =  pfNewMtl(arena); 

pfMUColor(mtl.  PFMTL  AMBIENT.  0.2f.  0.2f.  O.Of); 
pfMtlColor(mtl.  PFMTL  DIFFUSE.  l.Of,  l.Of,  O.Of); 
pfMUColor(mtl.  PFMTL_EMISSION.  O.Of,  O.Of.  O.Of). 
pfMUColor(mU.  PFMTL_SPECULAR,  O.Of,  O.Of.  O.Of); 
pfMtlAlpha(mtl.  1.0); 

pfGStateAttr(robotyeIlow _gstate.  PFSTATE_FRONTMTL.  mtl); 
pfGSetGStatefgset,  robotyellow _gstate); 

return  gset; 

} 

pfGeoSet* 

MakeLinklGSet(void) 

{ 

pfGeoSet  ♦gset; 
void  *arena; 

arena  =  pfGetSharedArenaO; 
gset  =  pfNewGSet(arena); 

pfGSetAttr(gsct.  PFGS_CCX)RD3.  PFGS_PER_VERTEX.  linklcoords.  NULL); 
pfGSetAttrfgset.  PFGS_NORMAL3,  PFGS_PER_,PRIM,  linklnorms,  NULL); 

pfGSetPrimTypefgset,  PFGS_QUADS); 
pfGSetNumPrimsfgset.  42); 

pfGSetGStatefgset.  robotyellow jgstate); 

return  gset; 

} 
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pfGeoSet* 

MakeLink2GSet(void) 

{ 

pfGeoSet  *gset; 
void  *arena; 

arena  »  pfGetSharedArena(); 
gsct  =  pfNe\vGSel(aiena); 

pfGSetAttr(gsct  PFGS_COORD3,  PFGS_PER  VERTEX  linklcooids,  NULL); 
pfGSetAttr(gset.  PFGS_NORMAL3.  PFGS.PeKpWH  Unk2nonns,  NULL); 

pfGSetPrimT>pe(gset.  PFGS_QUADS); 
pfGSetNutnPiims(gseL  91); 

pfGSetGState(gset,  robolyellow .gstate); 

return  gset; 

} 

pfGeoSet* 

MakeLink3  GSet(void) 

{ 

pfGeoSet  *gset; 
void  ‘arena; 

arena  ==  pfGetSharedArenaO; 
gset  -  pfNewGSet(arena); 

pfGSetAttrfgset,  PFGS_COORD3,  PFGS.PER  VERTEX  Unk3coords.  NULL); 
pfGSetAttrfgset,  PFGS_NORMAL3,  PFGS_Pro_PRIH  UnkSnorms,  NULL); 

pfGSetPrimTypeCgset.  PFGS_QUADS); 
pfGSetNumPrimsfgset,  103); 

pfGSetGStatefgset,  robotyellow ^^gstate); 

return  gset; 

} 
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pfGeoSct* 

MalG^oo(GSet(void) 

{ 

(^GeoSet  *tset; 
void  *areoa; 

arena  »  pfCetShaiedArenaQ; 
gset  »  pfNewGSet(aiena); 

pfGSctAttr(gset,  PFGS_CCX)RD3.  PFGS_PER_VERTEX.  feotawrf's.  NULL); 
pfGSetAttr(gset.  PFGS_NORMAL3,  PFGS_P^_PRIM,  footnorms,  aTILL); 

pfGSetPhmType(gset,  PFGS_TRISTRIPS); 
pfGSetNumPrimsCgset,  49); 
pfGSetPrimLengthsCgset.  footstriplengths); 

pfGSetGState(gset.  lobotyeliow  jgstate); 

return  gset; 

} 

pfGeoSet* 

MakeShaftGSet(void) 

{ 

pfGeoSet  ^gset; 

void  *arena; 

pfGeoState  *robotgray _gstate; 

pfMaterial  *mtl; 

arena  »  pfGetSharedArenaO; 

gset  =  pfNewGSet(atena); 

pfGSetAttr(gset.  PFGS_COORD3.  PFGS_PER_VERTEX,  shaftcoords,  NULL); 
pfGSetAttr(gset,  PFGS_NORMAL3,  PFGS_P^_PRIM.  shaftnorms,  NULL); 

pfGSetPrimTypeCgset,  PFGS_QUADS); 
pfGSetNutnPrimsCgset,  20); 

/*  set  up  material  */ 

robotgno^ _gstate  »  pfNewGState(atena); 

mtl « idNewMtl(atena); 

pfMdColor(mtl.  FF^nL.AMBIENT,  0. 1, 0. 1, 0. 1); 
pfMdColorCmtl,  PFNTIL.DIFFUSE,  0.2, 0.2, 0.2); 

|rfMtIColor(md,  PFMTL.EMISSION,  0.0, 0.0, 0.0); 
pfMtlCok>t(intI,  PFMTL.SPECULAR,  0.0, 0.0, 0.0); 
pfMtiAlphs^mtl,  1.0); 

pfGStateAtt^robotgray jgstate,  PFSTA1E_FRON7MIL,  mtl); 
pfGSetGStat^gset,  robotgray _gstate); 

return  gset; 

} 


124 


pfGiotip* 

MakeAquaRobotCpflXS  *JointDCS[6](4|) 

{ 

pfSCS  *LegAttacliSCS(6], 

*LiiiklSCSl61,  •Liiik2SCS(61.  *Link3SCS[61. 
*FootSCSl61; 

pfMatrix  rot.mat,  trans_inat; 

pfGroup  *AquaRobotGroup,  *BodyGroup(6|.  *LegGroup(6]; 
pfGeode  *geode_body. 

♦geodejinkl.  •geode_Iink2,  •geode_link3. 
*geode_shaft  *gcode_foot; 
int  i;  /•  loop  counter  */ 

/*  make  geodes  */ 
geode_body  =  pfNewGeodeO; 
pfAddGSet(geode_body.  MakeBodyGSetO); 

geode_linkl  =  pfNewGeodeQ; 
pfAddGSet(ge^_linkl,  MakeLinklGSetO); 

geode_Iink2  =  pfNewGeodeO; 
pfAddGSet(ge(^_link2.  MakeLinklGSetO); 

geodeJink3  =  pfNewGeodeO; 
pfAddGSet(ge(^Jink3,  MakeLink3GSetO); 

geode_foot  ®  pfNewGeodeO; 
pfAddGSet(geode_foot.  MakeFootGSetO); 

geode_shaft  =  pfNewGeodeO; 
pfAddGSetCge^.shaft,  MakeShaftGSetO); 

/*  Make  Parent  Group  *! 

AquaRobotGroup  =  pfNewGioupO; 

I*  Add  Structure  (6  segments)  */ 
for(i  =  0;  i  <  6;  i++) 

{ 

I*  rotate  to  segment  */ 

pfMakeRotMat(tot_mat,  i*60.0. 0.0, 0.0, 1.0); 
LegAttacbSCSU]  pfNewSCS(rot_mat); 
pfAddChild(AquaRobotGroup,  Le^ttacbSCS(i]); 

/*  add  body  sUce  */ 

BodyGroupCtl  ==  pfNewGtoupO; 
pfAddChild(LegAttacbSCS[i],  BodyGroup(il); 
pfAddCbild(BodyGroup(i],  geode_body); 
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/*  add  leg  */ 


/•  link  1  */ 

pfMakeTransMat(trans_mat,  LINKOLENGTH,  0.0. 0.0); 
LinklSCS(i]  *  pfNewSCS(trans_mat); 
pfAddChild(BodyGroup(i],  LinklSCS[i]); 
pfAddChild(LinklSCS(i].  geode_shaft); 

JointDCSIiKOl  =  pfNewDCSO; 
pfAddChild(LinklSCS(i],  JointDCS(iHOI): 
pfAddChild(  JointDCS(i]  [0],  geodejiiik  1 ); 

/•  link  2  *! 

pfMakeRotMat(rot_[nat,  -90.0,  1.0, 0.0. 0.0); 
pfMakeTransMat(trans_tnat.  LINK  1  LENGTH.  0.0.  0.0); 
pfPostMultMat(rot_niat,  trans_niat); 

Ltnk2SCS[i]  =  pfNewSCS(rot_mat); 
pfAddChild(JointDCS[il(0|,  Link2SCS(i|); 
pfAddChild(Link2SCS[i|,  geode_shaft); 

JointDCS[il(ll  =  pfNewDCSO; 
pfAddChild(Link2SCS(i].  JointDCS(i](  1  ]); 
pfAddChild(JointDCS[i]  [  1  ],  geode Jink2); 

/•  link  3  */ 

pfMakeTransMat(trans_mat,  LINK2LENGTH,  0.0, 0.0); 
Link3SCS(i|  -  pfNewSCS(Uans  mat); 
pfAddChild(JointDCS(i][l].  Link3SCS[i]); 
pfAddChild(Link3SCS(i],  geode.shafi); 

JointDCSlil(21  =  pfNewDCSO; 
pfAddChild(Link3SCS[i],  JointDCS[i][2|); 
pfAddCluld(JointDCS(i][2],  geode_li^); 

/*  foot  •/ 

pfMakeTransMat(trans_niat,  LINK3LENGTH,  0.0, 0.0); 
FootSCS{i]  =  pfNewSCS(tians_niat); 
pfAddChild(JointDCS(i](2],FootSCS[i]); 

JointDCS[i](3]  -  pfNewDCSO; 
pfAddChild(FootSCS(il.  JointDCS(i|(3|); 
pfAddChild(JointDCS{i](3],  geode  foot); 

} 

return  AquaRobotGroup; 
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#ifhdef_AQUA  H 
#define_AQUA_H 


#incli^e  <Perfonner/pf.h> 

#include  "misc.H" 

#include  "rigid_bod>.H" 

^include  "aquaJeg.H" 

t>-pedef  ngid_body  aquarobot_body; 

/*  mass  in  Kg.  ♦/ 

#defme  AQUA_BODY_MASS  500.0f 

/*  (Ix  ly  Iz)-Kg<m.  in  principal  axis  coordinates.  *! 

I*  assumes  solid  cylindrical  of  constant  density.  V 
#define  AQUA_BODY_HEIGHT  50.0f 
#define  AQUA_BODY_RADIUS  30.0f 
sutic  pfVec3  aqua_body_inertia  =  { 

//lx 

1.0fy4.0f  •  AQUA  BODY_MASS  ♦  AQUA  BODY  RADIUS  •  AQUA  BODY  RADIUS 
+  1.0f/12.0f  •  AQUA  BODY  MASS  •  AQUA  BODY_HEIGHT  *  AQUA  BODY  HEIGHT. 
//  ly 

1.0fi^4.0f  •  AQUA  BODY  MASS  *  AQUA  BODY  RADIUS  *  AQUA  BODY  RADIUS 
+  l.Of'll.Of  *  AQUA  BODY  MASS  *  AQUA  BODY  HEIGHT  *  AQUA  BODY  HEIGHT, 
I  Hz 

l.O^l.Of  •  AQUA_BODY_MASS  •  AQUA_BODY_RADIUS  *  AQUA_BODY_RADIUS}; 

/*  initial  position  and  orientation  in  world  coordinates.  */ 
ildefine  AZIMUTH_INIT  deg_to_rad(0.0f) 

#define  ELEVATldN_INrr  deg_to_rad(0.0f) 

Adeline  ROLL_INrr  deg_to  rad(O.Of) 

#define  X.INTT  O.Of 
#define  Y_INIT  O.Of 

#define  Z_INIT  sinf(default_angles(l))’*LINK2LENGTH  -  LINK3LENGTH 

/*  leg  attachment  angles.  */ 

^define  LEGIANGLE  deg_to_rad(O.Of) 

#define  LEG2ANGLE  deg_to_rad(60.0f) 

#define  LEG3 ANGLE  deg_to_rad(120.0f) 

#define  LEG4ANGLE  deg_to_rad(I80.0f) 

#define  LEGSANGLE  deg_to_rad(240.0f) 

#define  LEG6ANGLE  deg_to_rad(300.0f) 


class  aquarobot 

{ 

public; 

aquarobot_body  body; 

//private: 

aquajeg  legl,  Ieg2.  Ieg3,  Ieg4,  Ieg5.  Ieg6; 

private: 

void 

aquarobot:  :initJoint_access(); 
void 

update_forces_and_torques(); 


void 

update_legs(); 

public: 

//  External  access  to  joint  angles  for  passing  to  performer  model 
//  This  could  be  private  if  "updateJointDCS*  were  a  friend; 

//  however,  the  class  should  not  depend  on  needs  of  user, 
float*  joint_matrix(6|I41; 

public; 

aquarobot();body(AQUA  BODY  MASS,  aqua  body  inertia), 
leglOUEGlANGUE), 
leg2(LEG2ANGLE), 
leg3(LEG3ANGLE), 
ieg4(LEG4  ANGLE), 
leg5(LEG5ANGLE), 

leg6(LEG6 ANGLE)  {iiiitJoint_access();} 


void 

initialize(float  k  SPRING_K,  float  d  =  SPRING_D,  float  height  =  O.Of); 
void 

update_aquarobot(float  dt  -  O.Of); 

//  coordinate  transformation  routines 
void 

worid_to_aqua(pfVec3  destination,  pfVec3  source) 
{body.world_toJbo(fy(destituition,  source);} 


void 

aqua_to_world(pfVec3  destination,  pfVec3  source) 
{body.bodyjo  world(destination,  source);} 

}; 
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void 

updateJointOCS(aquarobot  robot.  pfDCS  *JointDCS{6](4]); 
#endif 
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//FUe"«qua.c'* 

^include  "aquaH" 

#include  <niath.h> 

/*  user  routines 

void 

aquarobot::initialize(float  k.  float  d,  float  height) 

{ 

body.tnove(AZIMLirH  INTT,  ELEVATION_INIT.  ROLL_INIT. 

X_IN1T.  Y_rNlf,  -fabs(height)+Z_INIT); 
pfSetVec3(body.vel_trans,  O.Of.  O.Of,  O.Of); 
pfCopyVec3(b^.vei_rot.  body.vel_trans); 
pfCopyVec3(body.accei_trans.  body.vel_trans); 
pfCopyVec3(body.accel_rot,  body.vel_trans); 
pfCopyVec3  (body,  forces,  body.vel_trans)', 
pfCopyVec3(body.torques,  body.vel_trans); 
body.start_  timerO; 
legl.init  i^rg(&body,  k,  d); 

Ieg2.init_ieg(£'body,  k,  d); 
leg3.init_leg(&body.  k.  d): 
leg4.initJeg(&body.  k. 
leg5.initJeg(&body .  k,  d); 
leg6.init_Ieg(&body,  k.  d); 
update  forces  and  torquesO; 

} 

void  aquarobot::update_aquarobot(float  dt) 

{ 

float  dt_  =  dt; 
if(dt<=  0.0) 

dt_  =  body.get_delta_tO;  H  default 

bodv.>ipdate_accelerationO; 

body.update_velocity(dtJ; 

body.update_H_matrix(dt_); 

body.update_position(dt_); 

//  body.update_velocity(dt_); 
update  JegsO; 

/*  This  is  done  last  as  it  also  updates  leg  positions  *! 

I*  leg  positions  depend  on  "new"  body  position!  */ 
update  forces_and_torquesO; 

} 
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/*  Internal  Routines  */ 


void 

aquarobot:  ;init Joint_access() 

{ 

//  for  use,  see  fit:  "update JointDCS*  below 
joint_matrix[0|[0]  =&legl.  link  1.  inboard Joint_angle; 
joint_niatrix(0|[l|  =  &legl.link2.inboardJoint_angle; 
joint_matrix(ojj2i  =  &legl.link3.inboardjoint_angie; 
joint_matrix(0)i3)  =  &Iegl.link4.H_matrix(0|(01; 

joint_matrix[  11(01  =  &leg2.  link  1.  inboard Joint_angie; 
joint_matrix(lljli  =  &ieg2.1ink2.inboardJoint_angle; 
joint_mathx(lj(2i  =  &leg2.1ink3.inboardJoint_angle: 
joint_niatrix(l](3]  =  &leg2.1ink4.H_matrix(01[0]; 

joint_mathx(21(01  =  &leg3.1inkl.inboardJoint_angle; 
joint_matrix(2j[ll  =  &leg3.1ink2.  inboard Joint_angle; 
joint_matrix(2j[2]  =  &leg3.iink3.  inboard Joint_angIe; 
joint_matrix(21[31  =  &leg3.1ink4.H_matrix(01(01; 

joint_niatrix(31I01  =  &leg4.1inkl.inboardJoint_angle: 
joint_tnatrix(31[ll  =  &leg4.1ink2.inboardJoint_angIe; 
joint_n)atrix[3][2|  =  &leg4.1ink3.  inboard Joint_angle; 
joint_matrix(3l|3j  =  &leg4.1ink4.H_matrix(01(0|; 

joint_inatrlx[4I[0] «  &leg5.Iinkl.inboardJoint_angle; 
joint_niatrix[4|[ll  =  &leg5.1ink2.inboardJoint_angle; 
joint_niatrix(4j[2j  =  &leg5.1ink3. inboard Joint_angle; 
joint_inatrix[4|[3]  =  &legS.link4.H_matrix(01[0]; 

joint_niatrix(51(01  =  &leg6.linkl.inboardJoint_angIe; 
joint_matrix[5j[ll  =  &leg6.1ink2.inboardJoint_angle; 
joint_matrix[Sj(21  =  &leg6.1ink3.inboardJoint_angle; 
joint_matrix{51(31  =  &leg6.1ink4.H_niatrix(01(01; 


void  aquarobot;  lupdate  forces  andjorquesQ 

{ 

pf5etVec3  (body. forces,  O.Of,  O.Of,  O.Of); 
pfSetVec3(body.torques,  O.Of,  O.Of,  O.Of); 
legl  .add_leg_forces_and_torquesO; 
Ieg2.add_leg_forces_and_torques0; 
Ieg3.add_leg_forccs_and_torques0; 
Ieg4.add_leg_forces_and_torques0; 
legs  .add_leg_forces_and_torquesO; 
leg6.addJeg_forces_and  torquesQ; 

} 
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voidaquarobot::update  legs() 

{ 

legl.updateJegO; 

Ieg2.updatejeg0; 

Ieg3.updatejeg0; 

leg4.updatejeg(); 

legS.updateJegO; 

Ieg6.update_lcg(); 

} 


/*  joint  angle  transfer  routine  */ 
void 

updateJointDCSCaquarobot  robot  pfDCS  *JointDCS(6|(4|) 

{ 

static  pfMatrix  m4  =  {{0,0.0,0}.{0.0,0,0},{0.0.0,0},{0.0.0.1}}; 
for(int  i=0;i<6;i++)  { 

//  rotate  first  three  joints 
forfint  j=OJ<3j-*-+)  { 
pfDCSRot(JointDCS(i][j], 

rad_to  deg(*robot.joint_matrix{il[jl),  O.Of.  O.Of); 

} 

//  equiv  to  rot(0x,-90y,02)  ♦  inverse(leg(i].link4.H_Matrix) 

//  accomplishes  DCS  such  that  link(x*axis)  i|  world(z-axis) 

m4{0][0]  =  (robot.joint_niatrix{i](3])(2]; 

m4(oj[lj  =  (robot.joint_matrix(ii(3i)|6i; 

m4(oj[2i  =  (robot.joint_matrix[ii[3])[101; 

m4|lj[oj  =  (rDbot.joint_inatrix(ij(3j)il]; 

m4[lj|l]  =  (robot.joint_niatrix(ii[3])|5i; 

m4[li[2j  (robot.joint_niatrix[ij|3])[9j; 

m4[2]|o]  =  •(>'obot.joint_niatrix(i](3])(0]; 

m4(2j[lj  =  •(robot.joint_matrix[ij(3j)[4j; 

m4[2][2]  =  -(robot.joint_niatrixii](3j)[8]; 

pfDCSMatrix(JointDCs7i][3],  m4); 

} 

} 
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//  file  ''aquajeg.h'* 


#iftidef_AQUA  LEG  H 
^define  _AQUA~LEgIh 

#include  <Perfonner/pf.h> 

#include  ’'rigid_body.H" 

#include  "aqua_link.H" 

^include  "misc.H" 

/•  initial  (default)  joint  angles.  */ 
static  pfVec3  default_angles  =  { 

/•  Odeg*/  O.Of, 

/*  45 deg*/  25.0f*PI_F/180.0f. 

/*  -135  deg  */  -1 15.0f  *  P1_F  /  ISO.Of}; 

/•  joint  spring  constant,  (default :  5,000,000  Kg-cm2/sec2  per  radian)  */ 

#define  SPRING_K  -5000000.0f 

/*  joint  spring  damping  constant,  (default ;  500.000  Kg-cm2/sec2  per  radian/sec)  •/ 
#define  SPRING_D  -500000.0f 

/•  AQUA  LEG  CLASS  */ 

class  aqua  leg 

( 

public; 

float  leg_attachment_angle; 

aqualinkO  linkO; 

aqualinkl  linkl; 

aqualink2  link2; 

aqualink3  link3; 

aqualink4  link4; 

boolean  motion_coinplete_flag; 

pfVec3  previous_foot_position; 

pfVec3  cuiTent_footjx)sition; 

boolean  foot_contact; 

float  spr_k; 

float  spr_d; 

public: 

aqua_leg(float  angle  =  O.Of);  leg_attachment_angle(angle) 

motion_complete_flag  =  TRUE; 
foot_contact  =  FALSE; 

} 


void 

init_lcg(rigid_body  ♦body,  float  k  =  SPRINGJL  float  d  =  SPRING_D); 
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void 

set_de£uili_anglesO; 

void 

updateJegO; 

void 

update_foot_pos(); 

void 

set_angles(float  joint  1,  float  joint!,  float  joint!); 
void 

set_angles(pfVec3  angles)  {set_angles(angles[OI,  angies(ll,  angles[21);} 
void 

jacobian(pfMathx  J); 
void 

inverseJacobian(pfMathx  JJnv); 
void 

joint_rates(pfVec3  rates); 
void 

FootPosFinJointl(pfVec3  footjtosjl,  pfVec3  footj»s_world); 
void 

aqua_inv_kin(pfVec3  joint_angles,  pfVec3  world_foot_pos); 
void 

add_leg_forces_and_torquesO; 

int 

new_contactO; 

int 

still  in_contact(pfVec3  leg;_foi:ce  body); 

>; 


#endif 
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//  file  ''aqua_leg.c" 


#include  <math.h> 

#include  "aquaJeg-H” 

/*  AQUA-ROBOT  INVERSE  KINEMATICS  ROUTINES  */ 

staUc  float  L2sqr  =  LINK2LENGTH  •  LINK2LENGTH; 
static  float  L3sqr  =  LINK3LENGTH  *  LINK3LENGTH; 

/*  routines  that  return  the  joint  angles  for  a  leg.  given  the  foot  position  */ 

float 

thetal(pfVec3  footjws) 

{ 

if  (foot  jwslO)  <  O.Of) 
return  (atan2f(-footjpos[l],  -footj)os[0|)); 
else 

return  (atan2f(foot_pos(l  I,  footj)os[01)); 

} 

float 

theta2(pfVec3  foot_pos,  float  hyp,  float  hyp  sqr) 

{ 

float  temp  =  asinf(foot  j)os(2)/hyp); 
if  (footjx)s(01  <  O.Of)  temp  =«  PI_F  -  temp; 

return  (acosf((L2sqr  +  hyp  sqr  -  L3sqr)  /  (2  •  LINK2LENGTH  *  hyp))  -  temp ); 

} 

float 

theta3(float  hyp_sqr) 

{ 

return  (acosf((L2sqr  +  L3sqr  -  hyp  sqr)  /  (2  •  LINK2LENGTH  •  LINK3LENGTH))  -  PI_F); 

} 

/*  supports  theta2  and  theta3  which  require  foot  position  with  respect  */ 

/*  to  joint2  position.  joint2  position  depends  on  thetal.  *! 

void 

FootPosFmJoint2(pfVec3  destination,  pfVec3  source) 

{ 

destination(0]  =  sqrtf(source[0]*source(0]  +  source[l]*sourcell]) 

-  LINKILENGTH; 
destinationfl]  =  O.Of; 
destination[2j »  source[21; 

} 
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/•  AQUA  LEG  CLASS  •/ 
void 

aqua  le^::initJeg(ngid_body  *body,  float  k.  float  d) 

{  '  ~  ” 
spr_k  =  k;  spr_d  =  d; 
foot_contact  =  0; 
linkO.inboardJink  =  bod>-; 
link  1.  inboard  Jink  =  &linkO; 

Iink2.  inboard  Jink  ==  &linkl; 
link3.  inboard  Jink  =  &link2; 
link4.inboard_link  =  &link3; 
set_default_angles(); 

} 

void 

aqua Jeg;  ;set_default_angles() 

{ 

set_angles(default_angles); 

update_foot_pos(); 

pfCopy  Vec3  (previous_foot_position,  curTent_foot jposition); 

} 

void 

aqua  leg;;update  ieg() 

{ 

pfCopy  Vec3  (previous_foot jposition,  curTent_foot_j)Osition); 
set_angles(linkl  .inboard Joint.angle, 
linkl.inboardjoint.angle, 

Iink3 .  inboard Joint_angle); 
update  foot_pos0; 

} 

void 

aqua  Jeg:  :update  footjposO 

{ 

if  (!foot_contact)  { 

current_foot_position[0]  ^  liiik4.H_iiiatrix[3](0]; 
cuiTent_foot_position(lj  =  liiik4.H_iiiatrix(3j|lj; 
curTent_foot_position{2j  =  liiik4.H  iiiattix[3j|2j; 

} 

} 
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void 

aqua  leg;;set  angles(float joint  1,  float  joint2,  float  joint3) 

{ 

iink0.rotateJiiik(leg^attachnient_angle); 
link  I .  rotate_Iiiik(joint  1 ); 
link2.rotateJink(joint2); 
link3 .  rotate_link0oint3); 

//  this  works  for  each  leg  in  2D,  but  world  3D  solution  requires(0.0) 
//  link4.rotate_link(-deg_to_rad(90.0f)  -  joint2  -  joint3): 
link4.rotate_link(0.0f)'. 

} 

void 

aqua  leg;  :jacobian(p{Matrix  J) 

{ 

pfVec3  row; 

float  angle.  SOI,  COl,  S2.  C2,  S23.  C23; 

#deflne  LI  LINKILENGTH 
^define  L2  LINK2LENGTH 
^define  L3  LINK3LENGTH 

angle  =  leg_attachinent_angle  +  link  1.  inboard Joint_angle; 

SOI  =  sinflangle); 

COl  =  cosflangle); 

angle  =  link2.inboardJoint_angle; 

S2  sinflangle); 

C2  =  cosflangle); 

angle  Iink3.  inboard Joint.angle; 

S23  sinflangle); 

C23  =  cosflangle); 

pfMakeldentMat(J); 

pfSetVec3(tow.  -SOI  *  (LI  +  L2*C2  +  L3*C23). 

-COl  •  (L2*S2  +  L3*S23). 

-COl  •  L3  •  S23); 
pfSetMatColVec3(J,  0,  row); 
pfSctVec3(iow,  COl  •  (LI  +  L2*C2  +  L3*C23). 

-SOI  *  (L2*S2  +  L3*S23). 

-SOI  •  L3  •  S23); 
pfSetMatColVec3(J,  1,  row); 
pfSetVec3(tow,  O.Of, 

-L2*C2-U*C23, 

-L3  *  C23); 

pfSetMatColVec3(J,  2,  tow); 

} 
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void 

aqua  leg::iiiverseJaoobian(pfMatrix  J  inv) 

( 

I^ifatrix  J; 
jacabian(J); 

|dInvertMat(J  inv.  J); 

} 

void 

aqua  leg::joint  rates(pfVec3  rates) 

{ 

pfMatrix  JJnv; 
pfVec3  trans_rates; 
pfVec3  omega; 
pfVec3  foot_r. 
pfVec3  FOt_rates; 
pfVec3  foot_rates; 

inverseJacobian(J_inv); 

pfScaleVec3(tians_rates,  -l.Of.  ((rigid_body  *)link0.inboardJink)>>vel_tnuis); 

pfCopyVec3 (omega,  ((rigid_bo<fy  •)link0.inboafd_link)->vel_rot); 

((rigid_body  *)link0.inboard_link)->world_to_body(foot_r.  current_foot_position); 
pfCrossVec3(rot_rates,  omega,  foot_r); 

pfSubVec3(  foot_iates,  tniiis_rates,  rot_rates); 
post  mu)t(rates.  J  inv,  foot  rates); 

void 

aqua  Ieg::FootPosFmJointl(pfVec3  foot j)osJl,pfVec3  foot jx>s  world) 

{ 

linkO.  world_to_body(foot jx»sJ  I,  foot_pos_world); 
footj)osjl(01  -=  LINKOLENGTH; 

) 

void 

aqua  leg;:aqua  inv  kin(pfVec3  joint  angles,  pfVec3  world_foot_pos) 

{  -  -  - 
pfVec3  footjointl,  footJointZ; 
float  hyp,  hyp_sqr. 

FootPosFmJointl(footJointl,  world_foot_pos); 

FootPosFmJoint2(footJoint2,  footjointl); 
hyp  =  pfLengthVec3;footJoint2); 
hyp_sqr  =  hyp  •  hyp; 

pf^tVec3(joint_ai^es,  thetal(footJointl), 

tlwta2(footJoint2,  hyp,  hyp_sqr), 
thm3(hyp_sqr)); 


void 

aqua  leg:;add_le|L-fi^ces  and  K>rques() 

{ 

if  (foot  contact  ||  oew_contactO} 

{ 

pfVec3  angles,  joint_torqiies.  damp,  foioes.  foot jMs; 
pfVbtrix  woi1c_niatrixl.  work_matrix2; 

aqua_inv_kin(angles,  current_footjposition): 
set_angles(angles): 

/*  get  spring  force  of  joints  */ 
pf5ubVec30oint_torques,  angles,  default_angles): 
pf5caieVec3(joint_torques,  spr_k,  joint_torques); 

/*  add  damping  *! 
joint_rates(damp); 
pfScaleVec3(damp.  spr_d,  damp); 
pfAddVec3(ioint_torques,  jointjorques,  damp); 

jacobian(work_matrix  1 ) ; 
pfrransposeMat(work_matrix2,  work_matrixl); 
pflnvertMat(work_matrixl,  work_matrix2); 
post_mult(forces,  work_matrixl.  jointjorques); 
pfScaleVec3(forces,  -1.0,  forces); 

if  (still  in  contact(forces)) 

{ 

((rigid_body  *)link0.inboard_link)->worldjoJbo<fy(footj)os,  current Jootjmsition); 
((rigid  body  *)link0.  inboard  link)->add  force  andjorques(footj)os,  forces); 

} 

} 

int 

aquajeg:  :new_contact() 

{ 

if  (currentjoot j»sition(2]  >  O.Of) 

{ 

current Jootj)osition[2]  =  O.Of; 
return  (foot  contact  =  IIIIIE); 

} 

else 

return  FALSE; 

} 


int 

aqua  leg;:^ll  in  contact(pfVec3  leg_force  body) 

{ 

pfVec3  leg_force_world; 

((rigid_body  •)link0.inboaTd_link)->body_to_worid(lcg_force_world.  leg_force_body); 
pfSubVec3(Ieg_forcc_world.  leg_force_world.  ((rigid_body  *)link0.inboard_link)->location); 
if  (leg  force  world[2T  >  O.Of) 

{ 

set_defaultiangles(); 
return  (foot_contact  =  FALSE); 

} 

else 

return  TRUE; 

} 
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//  flic  *'aquajink.h" 


#ifhdef  AQUA  LINK  H 
#define  _AQUA_LINK_H_ 

^include  <Perfonner/pf.h> 

#include  "rigid_body.H" 

/*  BASE  CLASSES  •/ 

class  linkipublic  rigid_body 

{ 

public: 

int  motion_liniit_nag; 

float  length_i_l; 

float  twist_i_l; 

float  inboardJoint_angle; 

float  inboardJoint_displacement: 

void*  inboard_link; 

pfMatrix  T_matrix; 

public: 

link(float  mass  =  l.Of.  pfVec3  moments  =  NULL);rigid_body(mass.  moments)! } 
void  update_T_matrix(); 
void  rotate(float  angle); 

}; 

class  rotaiy  link:public  link 

{ 

public: 

float  min  Joint_angle; 
float  fliaxJoim_angle; 

public: 

rotaiy_link(float  length  =  O.Of 

float  min_angle  =  O.Of, 

float  max_angle  =  O.Of, 

float  twist  =  O.Of, 

float  joint_angle  =*0.0f, 
float  joint_displaceinent »  O.Of, 
void*  inboard_link  =  0); 
void  rotate_link(float  angle); 

}; 
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/*  MODIFIED  DANEVir-HARTENBERG  LINK  COORDINATE  TRANSFORMATION  MATRIX  ♦/ 
void 

in(lh_inatrix(pfMatrtx  mdh, 

float  cosroiate,  float  sinrotate, 
float  costwist_i_L  float  sintwist_i_l. 
float  iength_i_  1 ,  float  translate): 


/•  AQUA-LINK  CLASSES  */ 

t*  link  lengths  *! 

#define  LINKOLENGTH  37.5f 
#defme  LINKILENGTH  20.0f 
#define  LINK2LENGTH  52.0f 
iWefine  LINK3LENGTH  I02.0f 
^define  LINK4LENGTH  3. Of 

/*  joint  limits  */ 

#deflne  JOINT  1  MIN  deg_to_rad(  -60.0f) 
#define  JOINTIMAX  deg_to_rad(  60.0f) 
#deflne  JOINT2MIN  deg_to_rad(-360.0f) 
itdefiac  JOINT2MAX  deg_to_rad(  360.00 
#deflne  JOINT3MIN  deg_to_rad(-360.00 
Mefine  JOINT3MAX  deg_to_rad(  360.00 
#deflne  JOINT4MIN  deg_to_rad(  -22.50 
^define  JOINT4MAX  deg_to_rad(  22.50 

class  aqualink0:public  rotary  link 

{ 

public: 

aqualinkOO; 

}; 


class  aqualinkLpublic  rotaiy_link 

{ 

public: 

aqualinklQ; 

}; 


class  aqualink2:public  rotaiy_link 

{ 

public: 

aqualink20; 

}; 
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class  aqualink3:public  rotaiyjink 

public: 

aqualink3(); 

J. 


class  aqualink4:  public  rotaryjink 

{ 

public: 

aqualink4(); 

}. 


#endif 


//  file  "aquajink-c" 


#include  <inath.h> 

#include  "aquaJink.H" 

#include  "misc.H" 

/*  BASE  CLASSES  */ 

void 

link:  :update_T_inatrix() 

{ 

float  sa  =  sinf(\nboard  joint_angle)-. 
float  ca  =  cosf(inboardJoint_angle); 
float  St  =  sinf(twist_i_l); 
float  ct  *  cosf(twist_i_l); 

mdh_matrix(T_matrix,  ca,  sa,  ct  st,  length_i_l.  inboardJoint_displacement); 

} 

void 

link:;rotate(float  angle) 

{ 

inboard  Joint_angle  =  angle; 
update_T_mauix(); 

//  multiplied  in  reverse  order  as  they  are  stored  as  transposes. 
pfMultMat(H  matrix,  T  matrix,  ((rigid  body  ♦)inboardJink)->H_matrix); 

rotary_link::rotary_link(float  length, 
float  min_angle, 
float  max_angle, 
float  twist, 
float  joint_angle, 
float  joint_displacement, 
void*  inboard_link_) 

{ 

length_i_l  =  length; 

minJoint_angle  =  inin_angle; 

maxJoint_angle  =  max_angle; 

twist_i_l  =  twist; 

inboard_joint_angle  =  jouit_angle; 
inboard Joint_displacement  -  joint_displacement; 
inboard_l<nk  =  inboanl_link_; 

pfMakeIdentMat(T  matrix); 

} 
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void 

rotary  Iiiik;;rotate  Iiiik(float  angle) 

{ 

I*  joint  stops  disabled, 
if  (angle  <  min  Joint  angle) 

{ 

rotate(minJoint_angle); 
motion  Jimit_flag  -  -1; 

} 

else  if  (angle  >  maxJoint_angle) 

rotate(maxJoint_angle); 
motion_limit_flag  =  1 ; 

} 

else 

{ 

V 

rotate(angle); 
motion_limit_f1ag  =  0; 

/• 

} 

•/ 

} 

void 

mdh_mathx  (pfMathx  mdh, ' 

float  ccsrotate,  float  sinrotate, 
float  cosiwist_i_l,  float  sintwistj_l, 
float  length  i  1,  float  translate) 

{ 

/•  col  1  */ 

mdh[0](0]  =  cosrotate; 
m<ih(li{0)  =  -  sinrotate; 
mdhpjJO)  =  O.Of; 
mdh(31[0I  =  length J_l; 

/•  col  2  V 

mdh[0][l|  =  sinrotate  *  costwistJ_l; 
mdh[l][l]  =  cosrotate  •  costwistJ_l; 
mdh(2]{l]  =  -  sintwistj_l; 
mdh(3](l]  =  -  sintwist_i_l  •  translate; 

I*  col  3  V 

mdh[0][2]  =  sinrotate  *  sintwist_i_l; 
mdh[l][2]  =  cosrotate  *  sintwist_i_l; 
mcth|2i|2]  =  costwistJ_l; 
mdhj3j[2j  =  costwist_i_l  *  translate; 

/•  col  4  •/ 

mdhl01l31  =  mdhi  11131  =  mdhpipi  =  O.Of; 
mdh[31I31  =  l.Of; 


/*  alternate  method  using  pf  functions 
pfVec3  col; 
pfMakeldentMat(mdh); 

pfSetVec3(col,  cosrotate,  sinrotate  •  costwist_i_l. 

sinrotate  •  sintwist_i_l); 
pfSetMatRowVec3(mdh,  0,  col); 
pfSetVec3(col.  -sinrotate,  cosrotate  *  costwist_i_l. 

cosrotate  *  sintwist_i_l); 
pfSetMatRowVcc3(mclh,  I,  coO; 
pfSetVec3(col,  0.0,  -sintwist_i_l,  costwist_i_l); 
pfSetMatRowVec3(mdh,  2,  col); 
pfSetVec3(col.  length_i_l,  -  sintwist_i_l  *  translate. 

costwist_i_l  •  translate): 
pfSetMatRo\vVec3(mdh.  3,  col); 

*/ 

} 

/•  AQUA-LINK  CLASSES  •/ 

aqualinkO:  :aqualinkO() 

{ 

maxJoint_angle  =  deg_to_rad(360.0f); 

} 

aqualink  1 :  ;aqualink  1 0 

{ 

length_i_l  *  LINKOLENGTH; 
minJoint_angle  =  JOINTIMIN; 
max  Joint  angle  =  JOINT  1  MAX; 

} 

aqualink2 ;  ;aqualink2() 

{ 

length_i_l  =LINK1LENGTH; 
twist J_1  =  deg_to_rad(-90.0f); 

min  Joint_angle  =  JOINT2MIN; 
maxJoint_angle  =  JOINT2MAX; 

} 

aqualink3 ;  ;aqualink30 

{ 

length J_1  =LINK2LENGTH; 

minJoint_angle  =  JOINT3MIN; 
maxJoint_angle  =  JOINT3MAX; 

} 
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aqualuik4:  ;a(iualiiik40 

{ 

lengthj.l  » LINK3LENGTH. 
tninJoint_anglc  =  J0INT4MIN; 
max  Joint  angle  “  JOINT4MAX; 

} 
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//file"rigid_body.H'* 


#ifhdef  RIGID  BODY 
#dcfinc  _RIGID_BODY_ 

#inclu(le  <Perfomier/pf.h> 

Wefinc  GRAVITY  980.0 

class  rigid  body 

{ 

public; 

pfVec3  location;  /•  The  vector  (x  y  z)  in  world  coordinates.  •/ 

pfVec3  vel_trans;  /*  The  vector  (u  v  w)  in  body  coordinates.  •/ 

pfVec3  vel_rot;  /*  The  vector  (p  q  r)  in  body  coordinates.  */ 

pfVec3  accel_trans;/*  The  vector  (u-dot  v-dot  w-dot).  */ 

pfVec3  accel_rot;  /•  The  vector  (p^ot  q-dot  r-dot).  */ 

pfVec3  forces;  /*  The  vector  (Fx  Fy  Fz)  in  body  coordinates.  */ 

pfVec3  torques;  /*  The  vector  (L  M  N)  in  body  coordinates.  •/ 

pfVec3  moments_;  /*  The  vector  (Ix  ly  Iz)  in  principal  axis  coordinates.  */ 

float  mass_;  /•  in  Kg.  •/ 

float  current_time; 

pfMatrix  H_matrix; 

public; 

rigid_body(float  mass.  pfVec3  moments  *  NULL); 
void 

move(float  azimuth,  float  elevation,  float  roll, 
float  X,  float  y.  float  z); 

float 

get_delta_t(); 

void 

start_timer(); 

/• 

void 

update_rigid  bodyO; 

void 

update_accelerationO; 

void 

update_velocity(float  dt); 
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void 

update_H_matnx(float  dt); 


void 

update  j>osition(float  dt), 
void 

\vorld_to_body{pfVec3  destination,  pfVec3  source); 
void 

body_to_world(pfVec3  destination.  pfVec3  source); 
void 

add_force_and_torques(pfVec3  r,  pfVec3  f); 


void 

homogeneous_transforni(pfMatrix  homo, 

float  azimuth,  float  elevation,  float  roll, 
float  X,  float  y,  float  z); 


void 

post_mult(pfVec3  destination,  pfMatrix  m,  pfVec3  source); 
#endif 
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//rae"rigid_body.C” 


#inclucle  "ripd_body.H" 

#include  "misc.H" 

rigid_body::ngid_body’(float  mass,  pfVec3  moments) 

{ 

location  (0)  =  location  [1]»  location  (2|  » 
vel_trans  (0)  =  vel_trans  [1]  =  vel_trans  (2)* 
vel_rot  (01  =  vel_rot  (1)  =  v-eLrot  (2)  * 
accel_trans(01  =  accel_trans{l)  =  accel_tfans(2|  = 
accel_rot  (0)  =  accel_rot  (1)  =  accel_rot  (21  = 
forces  (01  =  forces  (11  =  forces  (21  = 
torques  (01  “torques  (11  “torques  (21“0.0f; 
if  (moments  =  NULL) 

moments_(01  =  moments_(ll  =  nionients_(2I  “  O.Of. 
else 

pfCopyVec3(moments_,  moments); 
niass_  =  mass; 
pfMakeIdentMat(H_matrix); 
current  time  “O.Of; 

} 

void 

rigid_body;;move(float  azimuth,  float  elevation,  float  roll, 
float  X,  float  y,  float  z) 

{ 

homogeneous_transform(H_matrix,  azimuth,  elevation,  roll,  x,  y,  z); 
pfSetVec3(location,  x,  y.  z); 

} 

float 

rigid  body::get  delta  tQ 
{  " 

float  dt  =  0.05f; 
current_time  +=  dt; 
return  dt; 

} 

void 

rigid_body:;stait  timerQ 

{ 

current  time  “O.Of; 

} 
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I* 

void 

rigid  bo(fy:  .«qidate  rigid  bodyO 

( 

float  dt  “  get_ddtaj; 
upd«c_H_matrix(<U); 
update  j)osition(dt); 
update_velocity(dt); 
update_acceleraUon( ); 

} 

*/ 

void 

rigid  body;;update  accelerationO 

accel_trans(0]  =  vel_trans(l|  •  vel_rot(21  -  vel_trans(21  *  vel_rot(l) 

+  forcesfO]  /  mass.  +  GRAVITY  •  H_inatrix[0)[21; 
accel_uans(l|  =  vel_trans(21  *  vel_rot{01  -  vel_transl01  •  vel_rot(2I 
+  forces!  1 1  /  inass_  +  GRAVITY  *  H_inatrix{  11(21; 
accel_trans(21  =  vel_trans(01  •  vel_rot(ll  -  vel_trans(ll  *  vel_rot[01 
+  forces{21  /  mass.  +  GRAVITY  *  H_inatrix(21(21; 
accei_rot{0|  = 

((moments_(ll  -  inonients_[21)  •  vel_rot(ll  *  veI_rot[21  +  torques(Ol) 
/  momentsJOl; 

accel_rot(ll  = 

(7tnonients_(21  -  momentsJOl)  *  vel_iot{21  *  vel_rot(01  +  torques(ll) 
/  momentsjll; 

accel_rot(21  = 

(7moments_(0l  -  momentsjll)  *  vel_rot(01  •  vel_rotfll  +  torques(21) 
/  momentsJ21; 


void 

rigid_body;:update_velocity(float  dt) 

{ 

pfVec3  dv; 

pfScaleVec3(dv,  dt,  accel_tians); 
pfAddVec3(vel_trans,  vel_traiis,  dv); 
pfScaleVec3(dv,  dt,  accel_rot); 
pfAddVec3(vel_rot,  vel_rot,  dv); 

} 
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void 

rigid  bo(fy;;update  H  matrixCfloat  dt) 

{ 

pfMatrix  homo; 

homogeneous_tiaiisfonn(homo,  dt  *  vel_rot(2]. 
dt  •  vel_rot(lJ, 
dt  •  vel_tot(oi, 
dt  *  vel_trans(0), 
dt  •  vel_trans(l|, 
dt  *  vel_trans(2]); 
pfPreMultMat(H_inatrix,  homo); 

} 

void 

rigid  body::update_position(floatdt) 

{ 

pfGetMatRowVec3(H_inatrix.  3.  location); 

} 

void 

rigid  body::world  to_body(pfVec3  destination,  pA^ec3  source) 
{  ' 

pfMatrix  inv_H; 
pflnveitMat(inv_H.  H_matrix); 
post  mult(destination,  inv  H.  souiceO; 

} 

void 

rigid  bo(fy:;body  to  world(pfVec3  destination,  pfVec3  source) 
post_mult(destination,  H_matrix,  source); 

} 

void 

rigid_body;:add  force  and  torques(pfVec3  r,  pfVec3  f) 

{ 

pfVec3  t; 

pfCrossVec3(t,  r,  0; 
pfAddVec3(torques,  torques,  t); 
pfAddVec3(forces,  forces,  f); 

} 


void 

hotnogeneous_transform(pfMatrix  homo. 

float  azimuth,  float  elevation,  float  roll, 
float  X,  float  y,  float  z) 

{ 

float  sz,  cz.  sy.  cy,  sx,  cx; 

pfSinCos(rad_to_deg(azimuth),  &sz.  &cz); 
pfSinCos(rad_to_deg(elevation).  &sy.  &c>); 
pfSinCos(rad  to_deg(ron),  &sx.  &cx); 

/*  col  I  */ 

homo(0||0|  =  cz*cy; 
homo[l|(0|  =  cz*sy*sx  -  sz*cx; 
homo{2](0|  =  cz*sy*cx  +  sz*sx; 
homo[31[0]  =  x; 

/•  col  2  */ 
homo[0](l|  =  sz*cy; 
homo(ll(l|  =  cz*cx  +  sz*sy*sx; 
homo(2Ull  =  sz*sy*cx  +  cz*sx; 
homo(3][li  =  y; 

/•  col  3  */ 
homo(0|(2|  =  -sy; 
homo(lI[2|  =  cy*sx; 
homo(2i|2]  =  cy*cx; 
homo(3i[2j  =  z; 

/•  col  4  •/ 

homo(01l31  “  homo(ll(31 »  homo(2U3l  =  O.Of; 
homoi3i|3)  =  l.Of; 

} 

void 

post_muit(pfVec3  destination,  pfMatrix  m.  pfVec3  source) 

{ 

destination(0|  =  source[0]  *  m[0|[0]  +  source[l|  *  m[l](0]  + 
source[21  •  mUUO]  +  m(31[01; 

destination(l]  =  source(0]  *  m(0][l|  -i-  source(l]  *  m[ll[l]  + 
source(2|  *  m(21(ll  +  m(3)[ll; 

destination(2]  =  source(0]  *  m[0](2]  +  source(l]  *  m(l|[2]  + 
source[2]  «  m[21(2]  +  m(3|(2|; 

} 
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//  file  '*niisc.H'* 


#ifhdef_MY_MISC_ 

#define  _MY_MISC_ 

/*  type  BOOLEAN  */ 

typedef  int  boolean; 

#ifndef  TRUE 
#define  TRUE  1 
#€ndif 

#ifndef  FALSE 
#define  FALSE  0 
#endif 

#definePI  3.14159265358979323846 
#defmePI_F3.14159f 

/*  angle  measurement  conversions  */ 

float 

deg_to_rad(float  deg); 
float 

rad_to_deg(float  rad); 
double 

deg_to_rad(double  deg); 
double 

rad jo_deg(double  tad); 

#endif 
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//  file  ••misc.C" 


#include  "misc.H" 

/*  angle  measurement  conversions  */ 
float 

deg_to_rad{float  deg) 

{ 

const  float  rad_per_deg  =  P1_F  /  ISO.Of; 
return  (deg  *  rad_per_deg); 

float 

rad_to_deg(float  rad) 

{ 

const  float  deg_per_rad  =  ISO.Of  /  P1_F; 
return  (rad  •  deg  per  rad); 

} 

double 

deg_to_rad(double  deg) 

{ 

const  double  rad_per_deg  =  PI  / 180.0; 
return  (deg  •  rad_per  deg); 

} 

double 

rad  to  deg(double  rad) 

{ 

const  double  degjter  rad  =  180.0  /  PI; 
return  (rad  *  deg_per  rad); 

} 
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